forked from bartvdbraak/blender
Minor fixes in Bullet/constraint solving
Should make generic 6DOF constraint more useable, and rigid body stacking more stable (warmstarting was accidently switched off) If time allows, a few more minor last-minute 2.49 fixes might follow. Check out http://bulletphysics.com/constraintsTutorial.blend
This commit is contained in:
Erwin Coumans
parent
2de8f6e328
commit
a96ce9453f
@ -22,12 +22,13 @@ Written by: Marcus Hennix
|
||||
#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)
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
btConeTwistConstraint::btConeTwistConstraint()
|
||||
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
|
||||
@ -63,13 +64,13 @@ void btConeTwistConstraint::init()
|
||||
m_bMotorEnabled = false;
|
||||
m_maxMotorImpulse = btScalar(-1);
|
||||
|
||||
setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
|
||||
setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
|
||||
m_damping = btScalar(0.01);
|
||||
m_fixThresh = CONETWIST_DEF_FIX_THRESH;
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
@ -99,9 +100,9 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
info->nub--;
|
||||
}
|
||||
}
|
||||
} // btConeTwistConstraint::getInfo1()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
@ -230,7 +231,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
}
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btConeTwistConstraint::buildJacobian()
|
||||
{
|
||||
@ -239,6 +240,7 @@ void btConeTwistConstraint::buildJacobian()
|
||||
m_appliedImpulse = btScalar(0.);
|
||||
m_accTwistLimitImpulse = btScalar(0.);
|
||||
m_accSwingLimitImpulse = btScalar(0.);
|
||||
m_accMotorImpulse = btVector3(0.,0.,0.);
|
||||
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
@ -277,7 +279,7 @@ void btConeTwistConstraint::buildJacobian()
|
||||
}
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
@ -406,10 +408,10 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
|
||||
|
||||
}
|
||||
}
|
||||
else // no motor: do a little damping
|
||||
else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
|
||||
{
|
||||
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
|
||||
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
|
||||
btVector3 angVelA; bodyA.getAngularVelocity(angVelA);
|
||||
btVector3 angVelB; bodyB.getAngularVelocity(angVelB);
|
||||
btVector3 relVel = angVelB - angVelA;
|
||||
if (relVel.length2() > SIMD_EPSILON)
|
||||
{
|
||||
@ -490,7 +492,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
|
||||
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btConeTwistConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
@ -498,7 +500,7 @@ void btConeTwistConstraint::updateRHS(btScalar timeStep)
|
||||
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btConeTwistConstraint::calcAngleInfo()
|
||||
{
|
||||
@ -584,12 +586,12 @@ void btConeTwistConstraint::calcAngleInfo()
|
||||
m_twistAxis.normalize();
|
||||
}
|
||||
}
|
||||
} // btConeTwistConstraint::calcAngleInfo()
|
||||
}
|
||||
|
||||
|
||||
static btVector3 vTwist(1,0,0); // twist axis in constraint's space
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btConeTwistConstraint::calcAngleInfo2()
|
||||
{
|
||||
@ -597,13 +599,34 @@ void btConeTwistConstraint::calcAngleInfo2()
|
||||
m_twistLimitSign = btScalar(0.);
|
||||
m_solveTwistLimit = false;
|
||||
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
|
||||
// and motor target m_qTarget is within constraint limits
|
||||
// TODO : split rotation to pure swing and pure twist
|
||||
// compute desired transforms in world
|
||||
btTransform trPose(m_qTarget);
|
||||
btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame;
|
||||
btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame;
|
||||
btTransform trDeltaAB = trB * trPose * trA.inverse();
|
||||
btQuaternion qDeltaAB = trDeltaAB.getRotation();
|
||||
btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
|
||||
m_swingAxis = swingAxis;
|
||||
m_swingAxis.normalize();
|
||||
m_swingCorrection = qDeltaAB.getAngle();
|
||||
if(!btFuzzyZero(m_swingCorrection))
|
||||
{
|
||||
m_solveSwingLimit = true;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
// 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();
|
||||
@ -756,7 +779,7 @@ void btConeTwistConstraint::calcAngleInfo2()
|
||||
m_twistAngle = btScalar(0.f);
|
||||
}
|
||||
}
|
||||
} // btConeTwistConstraint::calcAngleInfo2()
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -982,8 +1005,5 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
|
||||
@ -17,6 +17,22 @@ Written by: Marcus Hennix
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Overview:
|
||||
|
||||
btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
|
||||
It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
|
||||
It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
|
||||
Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
|
||||
(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
|
||||
|
||||
In the contraint's frame of reference:
|
||||
twist is along the x-axis,
|
||||
and swing 1 and 2 are along the z and y axes respectively.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef CONETWISTCONSTRAINT_H
|
||||
#define CONETWISTCONSTRAINT_H
|
||||
|
||||
@ -141,6 +157,17 @@ public:
|
||||
};
|
||||
}
|
||||
|
||||
// setLimit(), a few notes:
|
||||
// _softness:
|
||||
// 0->1, recommend ~0.8->1.
|
||||
// describes % of limits where movement is free.
|
||||
// beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
|
||||
// _biasFactor:
|
||||
// 0->1?, recommend 0.3 +/-0.3 or so.
|
||||
// strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
|
||||
// __relaxationFactor:
|
||||
// 0->1, recommend to stay near 1.
|
||||
// the lower the value, the less the constraint will fight velocities which violate the angular limits.
|
||||
void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
|
||||
{
|
||||
m_swingSpan1 = _swingSpan1;
|
||||
|
||||
@ -26,7 +26,7 @@ http://gimpact.sf.net
|
||||
|
||||
|
||||
#define D6_USE_OBSOLETE_METHOD false
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
btGeneric6DofConstraint::btGeneric6DofConstraint()
|
||||
:btTypedConstraint(D6_CONSTRAINT_TYPE),
|
||||
@ -35,7 +35,7 @@ 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)
|
||||
@ -46,12 +46,12 @@ 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)
|
||||
@ -61,7 +61,7 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
|
||||
return mat[i][j];
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
|
||||
bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
|
||||
@ -129,7 +129,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
|
||||
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
btScalar btRotationalLimitMotor::solveAngularLimits(
|
||||
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
|
||||
@ -191,8 +191,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
|
||||
|
||||
|
||||
// sort with accumulated impulses
|
||||
btScalar lo = btScalar(-1e30);
|
||||
btScalar hi = btScalar(1e30);
|
||||
btScalar lo = btScalar(-BT_LARGE_FLOAT);
|
||||
btScalar hi = btScalar(BT_LARGE_FLOAT);
|
||||
|
||||
btScalar oldaccumImpulse = m_accumulatedImpulse;
|
||||
btScalar sum = oldaccumImpulse + clippedMotorImpulse;
|
||||
@ -249,9 +249,9 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu
|
||||
m_currentLimit[limitIndex] = 0;//Free from violation
|
||||
m_currentLimitError[limitIndex] = btScalar(0.f);
|
||||
return 0;
|
||||
} // btTranslationalLimitMotor::testLimitValue()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
btScalar btTranslationalLimitMotor::solveLinearAxis(
|
||||
btScalar timeStep,
|
||||
@ -283,8 +283,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
|
||||
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
|
||||
btScalar lo = btScalar(-1e30);
|
||||
btScalar hi = btScalar(1e30);
|
||||
btScalar lo = btScalar(-BT_LARGE_FLOAT);
|
||||
btScalar hi = btScalar(BT_LARGE_FLOAT);
|
||||
|
||||
btScalar minLimit = m_lowerLimit[limit_index];
|
||||
btScalar maxLimit = m_upperLimit[limit_index];
|
||||
@ -372,7 +372,7 @@ void btGeneric6DofConstraint::calculateAngleInfo()
|
||||
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::calculateTransforms()
|
||||
{
|
||||
@ -382,7 +382,7 @@ void btGeneric6DofConstraint::calculateTransforms()
|
||||
calculateAngleInfo();
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::buildLinearJacobian(
|
||||
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
|
||||
@ -400,7 +400,7 @@ void btGeneric6DofConstraint::buildLinearJacobian(
|
||||
m_rbB.getInvMass());
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::buildAngularJacobian(
|
||||
btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
|
||||
@ -413,7 +413,7 @@ void btGeneric6DofConstraint::buildAngularJacobian(
|
||||
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
|
||||
{
|
||||
@ -423,7 +423,7 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
|
||||
return m_angularLimits[axis_index].needApplyTorques();
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::buildJacobian()
|
||||
{
|
||||
@ -483,7 +483,7 @@ void btGeneric6DofConstraint::buildJacobian()
|
||||
}
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
@ -519,7 +519,7 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
}
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
@ -528,7 +528,7 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
setAngularLimits(info, row);
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
|
||||
{
|
||||
@ -559,7 +559,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
|
||||
return row;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
|
||||
{
|
||||
@ -582,7 +582,7 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
|
||||
return row;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
@ -643,7 +643,7 @@ void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv
|
||||
}
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
@ -651,21 +651,21 @@ void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
|
||||
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
|
||||
{
|
||||
return m_calculatedAxis[axis_index];
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
|
||||
{
|
||||
return m_calculatedAxisAngleDiff[axis_index];
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::calcAnchorPos(void)
|
||||
{
|
||||
@ -684,9 +684,9 @@ void btGeneric6DofConstraint::calcAnchorPos(void)
|
||||
const btVector3& pB = m_calculatedTransformB.getOrigin();
|
||||
m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
|
||||
return;
|
||||
} // btGeneric6DofConstraint::calcAnchorPos()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btGeneric6DofConstraint::calculateLinearInfo()
|
||||
{
|
||||
@ -696,9 +696,9 @@ void btGeneric6DofConstraint::calculateLinearInfo()
|
||||
{
|
||||
m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
|
||||
}
|
||||
} // btGeneric6DofConstraint::calculateLinearInfo()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
int btGeneric6DofConstraint::get_limit_motor_info2(
|
||||
btRotationalLimitMotor * limot,
|
||||
@ -824,6 +824,6 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
|
||||
else return 0;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
|
||||
|
||||
@ -477,4 +477,5 @@ public:
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
2
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
vendored
2
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
vendored
@ -490,7 +490,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
||||
|
||||
|
||||
///warm starting (or zero if disabled)
|
||||
if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
|
||||
@ -18,14 +18,14 @@ Added by Roman Ponomarev (rponom@gmail.com)
|
||||
April 04, 2008
|
||||
*/
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
#include "btSliderConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
#include <new>
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void btSliderConstraint::initParams()
|
||||
{
|
||||
@ -62,9 +62,9 @@ void btSliderConstraint::initParams()
|
||||
m_maxAngMotorForce = btScalar(0.);
|
||||
m_accumulatedAngMotorImpulse = btScalar(0.0);
|
||||
|
||||
} // btSliderConstraint::initParams()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
btSliderConstraint::btSliderConstraint()
|
||||
:btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
|
||||
@ -73,9 +73,9 @@ btSliderConstraint::btSliderConstraint()
|
||||
// m_useSolveConstraintObsolete(true)
|
||||
{
|
||||
initParams();
|
||||
} // btSliderConstraint::btSliderConstraint()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
|
||||
: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
|
||||
@ -86,9 +86,25 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const
|
||||
// m_useSolveConstraintObsolete(true)
|
||||
{
|
||||
initParams();
|
||||
} // btSliderConstraint::btSliderConstraint()
|
||||
}
|
||||
|
||||
|
||||
static btRigidBody s_fixed(0, 0, 0);
|
||||
btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
|
||||
: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, s_fixed, rbB)
|
||||
,
|
||||
m_frameInB(frameInB),
|
||||
m_useLinearReferenceFrameA(useLinearReferenceFrameB),
|
||||
m_useSolveConstraintObsolete(false)
|
||||
// m_useSolveConstraintObsolete(true)
|
||||
{
|
||||
///not providing rigidbody B means implicitly using worldspace for body B
|
||||
// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
|
||||
|
||||
initParams();
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::buildJacobian()
|
||||
{
|
||||
@ -104,9 +120,9 @@ void btSliderConstraint::buildJacobian()
|
||||
{
|
||||
buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
|
||||
}
|
||||
} // btSliderConstraint::buildJacobian()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
|
||||
{
|
||||
@ -159,9 +175,9 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
|
||||
// clear accumulator for motors
|
||||
m_accumulatedLinMotorImpulse = btScalar(0.0);
|
||||
m_accumulatedAngMotorImpulse = btScalar(0.0);
|
||||
} // btSliderConstraint::buildJacobianInt()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::getInfo1(btConstraintInfo1* info)
|
||||
{
|
||||
@ -189,9 +205,9 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info)
|
||||
info->nub--;
|
||||
}
|
||||
}
|
||||
} // btSliderConstraint::getInfo1()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::getInfo2(btConstraintInfo2* info)
|
||||
{
|
||||
@ -499,9 +515,9 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info)
|
||||
info->m_constraintError[srow] *= getSoftnessLimAng();
|
||||
} // if(limit)
|
||||
} // if angular limit or powered
|
||||
} // btSliderConstraint::getInfo2()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
@ -517,9 +533,9 @@ void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBod
|
||||
solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
|
||||
}
|
||||
}
|
||||
} // btSliderConstraint::solveConstraint()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
|
||||
{
|
||||
@ -703,11 +719,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& body
|
||||
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
|
||||
}
|
||||
}
|
||||
} // btSliderConstraint::solveConstraint()
|
||||
}
|
||||
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::calculateTransforms(void){
|
||||
if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
|
||||
@ -740,9 +756,9 @@ void btSliderConstraint::calculateTransforms(void){
|
||||
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
|
||||
m_depth[i] = m_delta.dot(normalWorld);
|
||||
}
|
||||
} // btSliderConstraint::calculateTransforms()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::testLinLimits(void)
|
||||
{
|
||||
@ -769,9 +785,9 @@ void btSliderConstraint::testLinLimits(void)
|
||||
{
|
||||
m_depth[0] = btScalar(0.);
|
||||
}
|
||||
} // btSliderConstraint::testLinLimits()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void btSliderConstraint::testAngLimits(void)
|
||||
{
|
||||
@ -795,9 +811,9 @@ void btSliderConstraint::testAngLimits(void)
|
||||
m_solveAngLim = true;
|
||||
}
|
||||
}
|
||||
} // btSliderConstraint::testAngLimits()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
btVector3 btSliderConstraint::getAncorInA(void)
|
||||
{
|
||||
@ -805,13 +821,13 @@ btVector3 btSliderConstraint::getAncorInA(void)
|
||||
ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
|
||||
ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
|
||||
return ancorInA;
|
||||
} // btSliderConstraint::getAncorInA()
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
btVector3 btSliderConstraint::getAncorInB(void)
|
||||
{
|
||||
btVector3 ancorInB;
|
||||
ancorInB = m_frameInB.getOrigin();
|
||||
return ancorInB;
|
||||
} // btSliderConstraint::getAncorInB();
|
||||
}
|
||||
|
||||
@ -25,23 +25,23 @@ TODO:
|
||||
#ifndef SLIDER_CONSTRAINT_H
|
||||
#define SLIDER_CONSTRAINT_H
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
class btRigidBody;
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
class btSliderConstraint : public btTypedConstraint
|
||||
{
|
||||
@ -126,6 +126,7 @@ protected:
|
||||
public:
|
||||
// constructors
|
||||
btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
|
||||
btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
|
||||
btSliderConstraint();
|
||||
// overrides
|
||||
virtual void buildJacobian();
|
||||
@ -223,7 +224,7 @@ public:
|
||||
btVector3 getAncorInB(void);
|
||||
};
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
#endif //SLIDER_CONSTRAINT_H
|
||||
|
||||
|
||||
4
extern/bullet2/src/LinearMath/btScalar.h
vendored
4
extern/bullet2/src/LinearMath/btScalar.h
vendored
@ -168,8 +168,12 @@ inline int btGetVersion()
|
||||
///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
|
||||
#if defined(BT_USE_DOUBLE_PRECISION)
|
||||
typedef double btScalar;
|
||||
//this number could be bigger in double precision
|
||||
#define BT_LARGE_FLOAT 1e30
|
||||
#else
|
||||
typedef float btScalar;
|
||||
//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
|
||||
#define BT_LARGE_FLOAT 1e18f
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@ -1510,10 +1510,11 @@ static void draw_constraint (uiBlock *block, ListBase *list, bConstraint *con, s
|
||||
|
||||
uiDefBut(block, ROUNDBOX, B_DIFF, "", *xco-10, *yco-height, width+40,height-1, NULL, 5.0, 0.0, 12, rb_col, "");
|
||||
|
||||
uiDefButI(block, MENU, B_CONSTRAINT_TEST, "Joint Types%t|Ball%x1|Hinge%x2|Cone Twist%x4|Generic (experimental)%x12",//|Extra Force%x6",
|
||||
uiDefButI(block, MENU, B_CONSTRAINT_TEST, "Joint Types%t|Ball%x1|Hinge%x2|Generic 6DOF%x12",//|Extra Force%x6",
|
||||
//uiDefButI(block, MENU, B_CONSTRAINT_TEST, "Joint Types%t|Ball%x1|Hinge%x2|Cone Twist%x4|Generic 6DOF%x12",//|Extra Force%x6",
|
||||
*xco, *yco-25, 150, 18, &data->type, 0, 0, 0, 0, "Choose the joint type");
|
||||
|
||||
uiDefButBitS(block, TOG, CONSTRAINT_DISABLE_LINKED_COLLISION, B_CONSTRAINT_TEST, "No Col.", *xco+155, *yco-25, 111, 18, &data->flag, 0, 24, 0, 0, "Disable Collision Between Linked Bodies");
|
||||
uiDefButBitS(block, TOG, CONSTRAINT_DISABLE_LINKED_COLLISION, B_CONSTRAINT_TEST, "No Collision", *xco+155, *yco-25, 111, 18, &data->flag, 0, 24, 0, 0, "Disable Collision Between Linked Bodies");
|
||||
|
||||
|
||||
uiDefIDPoinBut(block, test_obpoin_but, ID_OB, B_CONSTRAINT_CHANGETARGET, "toObject:", *xco, *yco-50, 130, 18, &data->tar, "Child Object");
|
||||
|
||||
Loading…
Reference in New Issue
Block a user