mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-18 21:10:05 +00:00
Add support for b3Generic6DofConstraint in Bullet, CPU only for now. Also the b3GpuDynamicsWorld supports conversion of this constraint.
This means, picking works both with and without holding SHIFT (rayTest is only implemented for spheres)
This commit is contained in:
erwin coumans
parent
f10eb86f55
commit
aa1c2db35a
@ -10,12 +10,14 @@
|
||||
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
|
||||
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h"
|
||||
#include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
|
||||
#include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
|
||||
#include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h"
|
||||
#include "Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h"
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
|
||||
#include "Bullet3Common/b3Logging.h"
|
||||
@ -68,6 +70,8 @@ int b3GpuDynamicsWorld::stepSimulation( btScalar timeStepUnused, int maxSubStep
|
||||
BT_PROFILE("stepSimulation");
|
||||
|
||||
{
|
||||
BT_PROFILE("sync constraints CPU");
|
||||
//todo: determine what data has changed, or perform copy on GPU?
|
||||
for (int i=0;i<m_constraints.size();i++)
|
||||
{
|
||||
btTypedConstraint* constraint = m_constraints[i];
|
||||
@ -88,6 +92,19 @@ int b3GpuDynamicsWorld::stepSimulation( btScalar timeStepUnused, int maxSubStep
|
||||
|
||||
break;
|
||||
};
|
||||
|
||||
case D6_CONSTRAINT_TYPE:
|
||||
{
|
||||
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
|
||||
b3Generic6DofConstraint* dof3 = (b3Generic6DofConstraint*) c;
|
||||
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
|
||||
|
||||
b3Transform frameInA = (b3Transform&) dof2->getFrameOffsetA();
|
||||
b3Transform frameInB = (b3Transform&) dof2->getFrameOffsetB();
|
||||
dof3->setFrames(frameInA,frameInB,bodiesCL);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
}
|
||||
@ -572,19 +589,14 @@ void b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disab
|
||||
case POINT2POINT_CONSTRAINT_TYPE:
|
||||
{
|
||||
btPoint2PointConstraint* p = (btPoint2PointConstraint*) constraint;
|
||||
|
||||
int rbA = p->getRigidBodyA().getUserIndex();
|
||||
int rbB = p->getRigidBodyB().getUserIndex();
|
||||
|
||||
btVector3 pivotInB = p->getPivotInB();
|
||||
|
||||
if (rbB<=0)
|
||||
{
|
||||
|
||||
pivotInB = p->getRigidBodyA().getWorldTransform()*p->getPivotInA();
|
||||
rbB = m_staticBody->getUserIndex();
|
||||
}
|
||||
|
||||
if (rbA>=0 && rbB>=0)
|
||||
{
|
||||
b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
|
||||
@ -593,11 +605,67 @@ void b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disab
|
||||
m_rigidBodyPipeline->addConstraint(p2p);
|
||||
} else
|
||||
{
|
||||
|
||||
b3Error("invalid body index in addConstraint.\n");
|
||||
b3Error("invalid body index in addConstraint,b3Point2PointConstraint\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case D6_CONSTRAINT_TYPE:
|
||||
{
|
||||
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
|
||||
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
|
||||
|
||||
int rbA = dof2->getRigidBodyA().getUserIndex();
|
||||
int rbB = dof2->getRigidBodyB().getUserIndex();
|
||||
|
||||
btTransform frameInA = dof2->getFrameOffsetB();
|
||||
btTransform frameInB = dof2->getFrameOffsetB();
|
||||
|
||||
if (rbA<=0)
|
||||
{
|
||||
frameInA = dof2->getRigidBodyB().getWorldTransform()*dof2->getFrameOffsetB();
|
||||
rbA = m_staticBody->getUserIndex();
|
||||
}
|
||||
|
||||
if (rbB<=0)
|
||||
{
|
||||
frameInB = dof2->getRigidBodyA().getWorldTransform()*dof2->getFrameOffsetA();
|
||||
rbB = m_staticBody->getUserIndex();
|
||||
}
|
||||
if (rbA>=0 && rbB>=0)
|
||||
{
|
||||
b3Generic6DofConstraint* dof3 = new b3Generic6DofConstraint(rbA,rbB,(b3Transform&)frameInA,(b3Transform&)frameInB,false,bodiesCL);//(();//(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
|
||||
{
|
||||
btVector3 limit(0,0,0);
|
||||
dof2->getLinearLowerLimit(limit);
|
||||
dof3->setLinearLowerLimit((const b3Vector3&)limit);
|
||||
|
||||
dof2->setLinearUpperLimit(limit);
|
||||
dof3->setLinearUpperLimit((const b3Vector3&)limit);
|
||||
|
||||
dof2->setAngularLowerLimit(limit);
|
||||
dof3->setAngularLowerLimit((const b3Vector3&)limit);
|
||||
|
||||
dof2->setAngularUpperLimit(limit);
|
||||
dof3->setAngularUpperLimit((const b3Vector3&)limit);
|
||||
/* for (int i=0;i<6;i++)
|
||||
{
|
||||
dof3->setParam(BT_CONSTRAINT_STOP_CFM,dof2->getParam(BT_CONSTRAINT_STOP_CFM,i),i);
|
||||
dof3->setParam(BT_CONSTRAINT_STOP_ERP,dof2->getParam(BT_CONSTRAINT_STOP_ERP,i),i);
|
||||
}
|
||||
*/
|
||||
dof3->setBreakingImpulseThreshold(dof2->getBreakingImpulseThreshold());
|
||||
}
|
||||
// p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
|
||||
constraint->setUserConstraintPtr(dof3);
|
||||
m_rigidBodyPipeline->addConstraint(dof3);
|
||||
} else
|
||||
{
|
||||
b3Error("invalid body index in addConstraint, btGeneric6DofConstraint.\n");
|
||||
}
|
||||
|
||||
// b3Generic6DofConstraint
|
||||
break;
|
||||
}
|
||||
default:
|
||||
b3Warning("Warning: b3GpuDynamicsWorld::addConstraint with unsupported constraint type\n");
|
||||
};
|
||||
|
||||
823
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
Normal file
823
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
Normal file
@ -0,0 +1,823 @@
|
||||
/*
|
||||
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 "b3Generic6DofConstraint.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
|
||||
|
||||
#include "Bullet3Common/b3TransformUtil.h"
|
||||
#include "Bullet3Common/b3TransformUtil.h"
|
||||
#include <new>
|
||||
|
||||
|
||||
|
||||
#define D6_USE_OBSOLETE_METHOD false
|
||||
#define D6_USE_FRAME_OFFSET true
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyCL* bodies)
|
||||
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
|
||||
, m_frameInA(frameInA)
|
||||
, m_frameInB(frameInB),
|
||||
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
|
||||
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
|
||||
m_flags(0),
|
||||
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
|
||||
{
|
||||
calculateTransforms(bodies);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#define GENERIC_D6_DISABLE_WARMSTARTING 1
|
||||
|
||||
|
||||
|
||||
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
|
||||
b3Scalar btGetMatrixElem(const b3Matrix3x3& 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 b3Matrix3x3& mat,b3Vector3& xyz);
|
||||
bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& 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
|
||||
//
|
||||
|
||||
b3Scalar fi = btGetMatrixElem(mat,2);
|
||||
if (fi < b3Scalar(1.0f))
|
||||
{
|
||||
if (fi > b3Scalar(-1.0f))
|
||||
{
|
||||
xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
|
||||
xyz[1] = b3Asin(btGetMatrixElem(mat,2));
|
||||
xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
|
||||
xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
|
||||
xyz[1] = -B3_HALF_PI;
|
||||
xyz[2] = b3Scalar(0.0);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
|
||||
xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
|
||||
xyz[1] = B3_HALF_PI;
|
||||
xyz[2] = 0.0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
//////////////////////////// b3RotationalLimitMotor ////////////////////////////////////
|
||||
|
||||
int b3RotationalLimitMotor::testLimitValue(b3Scalar 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;
|
||||
if(m_currentLimitError>B3_PI)
|
||||
m_currentLimitError-=B3_2_PI;
|
||||
else if(m_currentLimitError<-B3_PI)
|
||||
m_currentLimitError+=B3_2_PI;
|
||||
return 1;
|
||||
}
|
||||
else if (test_value> m_hiLimit)
|
||||
{
|
||||
m_currentLimit = 2;//High limit violation
|
||||
m_currentLimitError = test_value - m_hiLimit;
|
||||
if(m_currentLimitError>B3_PI)
|
||||
m_currentLimitError-=B3_2_PI;
|
||||
else if(m_currentLimitError<-B3_PI)
|
||||
m_currentLimitError+=B3_2_PI;
|
||||
return 2;
|
||||
};
|
||||
|
||||
m_currentLimit = 0;//Free from violation
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
|
||||
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
|
||||
|
||||
|
||||
int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
|
||||
{
|
||||
b3Scalar loLimit = m_lowerLimit[limitIndex];
|
||||
b3Scalar hiLimit = m_upperLimit[limitIndex];
|
||||
if(loLimit > hiLimit)
|
||||
{
|
||||
m_currentLimit[limitIndex] = 0;//Free from violation
|
||||
m_currentLimitError[limitIndex] = b3Scalar(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] = b3Scalar(0.f);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
|
||||
|
||||
void b3Generic6DofConstraint::calculateAngleInfo()
|
||||
{
|
||||
b3Matrix3x3 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.
|
||||
b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
|
||||
b3Vector3 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();
|
||||
|
||||
}
|
||||
|
||||
static b3Transform getCenterOfMassTransform(const b3RigidBodyCL& body)
|
||||
{
|
||||
b3Transform tr(body.m_quat,body.m_pos);
|
||||
return tr;
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyCL* bodies)
|
||||
{
|
||||
b3Transform transA;
|
||||
b3Transform transB;
|
||||
transA = getCenterOfMassTransform(bodies[m_rbA]);
|
||||
transB = getCenterOfMassTransform(bodies[m_rbB]);
|
||||
calculateTransforms(transA,transB,bodies);
|
||||
}
|
||||
|
||||
void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyCL* bodies)
|
||||
{
|
||||
m_calculatedTransformA = transA * m_frameInA;
|
||||
m_calculatedTransformB = transB * m_frameInB;
|
||||
calculateLinearInfo();
|
||||
calculateAngleInfo();
|
||||
if(m_useOffsetForConstraintFrame)
|
||||
{ // get weight factors depending on masses
|
||||
b3Scalar miA = bodies[m_rbA].m_invMass;
|
||||
b3Scalar miB = bodies[m_rbB].m_invMass;
|
||||
m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
|
||||
b3Scalar miS = miA + miB;
|
||||
if(miS > b3Scalar(0.f))
|
||||
{
|
||||
m_factA = miB / miS;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_factA = b3Scalar(0.5f);
|
||||
}
|
||||
m_factB = b3Scalar(1.0f) - m_factA;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
|
||||
{
|
||||
b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
|
||||
angle = b3AdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
|
||||
m_angularLimits[axis_index].m_currentPosition = angle;
|
||||
//test limits
|
||||
m_angularLimits[axis_index].testLimitValue(angle);
|
||||
return m_angularLimits[axis_index].needApplyTorques();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
info->m_numConstraintRows = 0;
|
||||
info->nub = 0;
|
||||
} else
|
||||
{
|
||||
//prepare constraint
|
||||
calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
|
||||
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 b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
info->m_numConstraintRows = 0;
|
||||
info->nub = 0;
|
||||
} else
|
||||
{
|
||||
//pre-allocate all 6
|
||||
info->m_numConstraintRows = 6;
|
||||
info->nub = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyCL* bodies)
|
||||
{
|
||||
b3Assert(!m_useSolveConstraintObsolete);
|
||||
|
||||
b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
|
||||
b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
|
||||
const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
|
||||
const b3Vector3& linVelB = bodies[m_rbB].m_linVel;
|
||||
const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
|
||||
const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
|
||||
|
||||
if(m_useOffsetForConstraintFrame)
|
||||
{ // for stability better to solve angular limits first
|
||||
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
}
|
||||
else
|
||||
{ // leave old version for compatibility
|
||||
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyCL* bodies)
|
||||
{
|
||||
|
||||
b3Assert(!m_useSolveConstraintObsolete);
|
||||
//prepare constraint
|
||||
calculateTransforms(transA,transB,bodies);
|
||||
|
||||
int i;
|
||||
for (i=0;i<3 ;i++ )
|
||||
{
|
||||
testAngularLimitMotor(i);
|
||||
}
|
||||
|
||||
if(m_useOffsetForConstraintFrame)
|
||||
{ // for stability better to solve angular limits first
|
||||
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
}
|
||||
else
|
||||
{ // leave old version for compatibility
|
||||
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
|
||||
{
|
||||
// int row = 0;
|
||||
//solve linear limits
|
||||
b3RotationalLimitMotor limot;
|
||||
for (int i=0;i<3 ;i++ )
|
||||
{
|
||||
if(m_linearLimits.needApplyForce(i))
|
||||
{ // re-use rotational motor code
|
||||
limot.m_bounce = b3Scalar(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_hiLimit = m_linearLimits.m_upperLimit[i];
|
||||
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
|
||||
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
|
||||
limot.m_maxLimitForce = b3Scalar(0.f);
|
||||
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
|
||||
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
|
||||
b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
|
||||
int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
|
||||
limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
|
||||
limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
|
||||
if(m_useOffsetForConstraintFrame)
|
||||
{
|
||||
int indx1 = (i + 1) % 3;
|
||||
int indx2 = (i + 2) % 3;
|
||||
int rotAllowed = 1; // rotations around orthos to current axis
|
||||
if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
|
||||
{
|
||||
rotAllowed = 0;
|
||||
}
|
||||
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
|
||||
}
|
||||
else
|
||||
{
|
||||
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
return row;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
|
||||
{
|
||||
b3Generic6DofConstraint * d6constraint = this;
|
||||
int row = row_offset;
|
||||
//solve angular limits
|
||||
for (int i=0;i<3 ;i++ )
|
||||
{
|
||||
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
|
||||
{
|
||||
b3Vector3 axis = d6constraint->getAxis(i);
|
||||
int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
if(!(flags & B3_6DOF_FLAGS_CFM_NORM))
|
||||
{
|
||||
m_angularLimits[i].m_normalCFM = info->cfm[0];
|
||||
}
|
||||
if(!(flags & B3_6DOF_FLAGS_CFM_STOP))
|
||||
{
|
||||
m_angularLimits[i].m_stopCFM = info->cfm[0];
|
||||
}
|
||||
if(!(flags & B3_6DOF_FLAGS_ERP_STOP))
|
||||
{
|
||||
m_angularLimits[i].m_stopERP = info->erp;
|
||||
}
|
||||
row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
|
||||
transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
|
||||
}
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyCL* bodies)
|
||||
{
|
||||
m_frameInA = frameA;
|
||||
m_frameInB = frameB;
|
||||
|
||||
calculateTransforms(bodies);
|
||||
}
|
||||
|
||||
|
||||
|
||||
b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
|
||||
{
|
||||
return m_calculatedAxis[axis_index];
|
||||
}
|
||||
|
||||
|
||||
b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
|
||||
{
|
||||
return m_calculatedLinearDiff[axisIndex];
|
||||
}
|
||||
|
||||
|
||||
b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
|
||||
{
|
||||
return m_calculatedAxisAngleDiff[axisIndex];
|
||||
}
|
||||
|
||||
|
||||
|
||||
void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyCL* bodies)
|
||||
{
|
||||
b3Scalar imA = bodies[m_rbA].m_invMass;
|
||||
b3Scalar imB = bodies[m_rbB].m_invMass;
|
||||
b3Scalar weight;
|
||||
if(imB == b3Scalar(0.0))
|
||||
{
|
||||
weight = b3Scalar(1.0);
|
||||
}
|
||||
else
|
||||
{
|
||||
weight = imA / (imA + imB);
|
||||
}
|
||||
const b3Vector3& pA = m_calculatedTransformA.getOrigin();
|
||||
const b3Vector3& pB = m_calculatedTransformB.getOrigin();
|
||||
m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void b3Generic6DofConstraint::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 b3Generic6DofConstraint::get_limit_motor_info2(
|
||||
b3RotationalLimitMotor * limot,
|
||||
const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
|
||||
b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed)
|
||||
{
|
||||
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
|
||||
b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
|
||||
b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
|
||||
if (J1)
|
||||
{
|
||||
J1[srow+0] = ax1[0];
|
||||
J1[srow+1] = ax1[1];
|
||||
J1[srow+2] = ax1[2];
|
||||
}
|
||||
if (J2)
|
||||
{
|
||||
J2[srow+0] = -ax1[0];
|
||||
J2[srow+1] = -ax1[1];
|
||||
J2[srow+2] = -ax1[2];
|
||||
}
|
||||
if((!rotational))
|
||||
{
|
||||
if (m_useOffsetForConstraintFrame)
|
||||
{
|
||||
b3Vector3 tmpA, tmpB, relA, relB;
|
||||
// get vector from bodyB to frameB in WCS
|
||||
relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
|
||||
// get its projection to constraint axis
|
||||
b3Vector3 projB = ax1 * relB.dot(ax1);
|
||||
// get vector directed from bodyB to constraint axis (and orthogonal to it)
|
||||
b3Vector3 orthoB = relB - projB;
|
||||
// same for bodyA
|
||||
relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
|
||||
b3Vector3 projA = ax1 * relA.dot(ax1);
|
||||
b3Vector3 orthoA = relA - projA;
|
||||
// get desired offset between frames A and B along constraint axis
|
||||
b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
|
||||
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
|
||||
b3Vector3 totalDist = projA + ax1 * desiredOffs - projB;
|
||||
// get offset vectors relA and relB
|
||||
relA = orthoA + totalDist * m_factA;
|
||||
relB = orthoB - totalDist * m_factB;
|
||||
tmpA = relA.cross(ax1);
|
||||
tmpB = relB.cross(ax1);
|
||||
if(m_hasStaticBody && (!rotAllowed))
|
||||
{
|
||||
tmpA *= m_factA;
|
||||
tmpB *= m_factB;
|
||||
}
|
||||
int i;
|
||||
for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
|
||||
for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
|
||||
} else
|
||||
{
|
||||
b3Vector3 ltd; // Linear Torque Decoupling vector
|
||||
b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
|
||||
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() - transB.getOrigin();
|
||||
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] = b3Scalar(0.f);
|
||||
if (powered)
|
||||
{
|
||||
info->cfm[srow] = limot->m_normalCFM;
|
||||
if(!limit)
|
||||
{
|
||||
b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
|
||||
|
||||
b3Scalar mot_fact = getMotorFactor( limot->m_currentPosition,
|
||||
limot->m_loLimit,
|
||||
limot->m_hiLimit,
|
||||
tag_vel,
|
||||
info->fps * limot->m_stopERP);
|
||||
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)
|
||||
{
|
||||
b3Scalar k = info->fps * limot->m_stopERP;
|
||||
if(!rotational)
|
||||
{
|
||||
info->m_constraintError[srow] += k * limot->m_currentLimitError;
|
||||
}
|
||||
else
|
||||
{
|
||||
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
|
||||
}
|
||||
info->cfm[srow] = limot->m_stopCFM;
|
||||
if (limot->m_loLimit == limot->m_hiLimit)
|
||||
{ // limited low and high simultaneously
|
||||
info->m_lowerLimit[srow] = -B3_INFINITY;
|
||||
info->m_upperLimit[srow] = B3_INFINITY;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (limit == 1)
|
||||
{
|
||||
info->m_lowerLimit[srow] = 0;
|
||||
info->m_upperLimit[srow] = B3_INFINITY;
|
||||
}
|
||||
else
|
||||
{
|
||||
info->m_lowerLimit[srow] = -B3_INFINITY;
|
||||
info->m_upperLimit[srow] = 0;
|
||||
}
|
||||
// deal with bounce
|
||||
if (limot->m_bounce > 0)
|
||||
{
|
||||
// calculate joint velocity
|
||||
b3Scalar vel;
|
||||
if (rotational)
|
||||
{
|
||||
vel = angVelA.dot(ax1);
|
||||
//make sure that if no body -> angVelB == zero vec
|
||||
// if (body1)
|
||||
vel -= angVelB.dot(ax1);
|
||||
}
|
||||
else
|
||||
{
|
||||
vel = linVelA.dot(ax1);
|
||||
//make sure that if no body -> angVelB == zero vec
|
||||
// if (body1)
|
||||
vel -= linVelB.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)
|
||||
{
|
||||
b3Scalar newc = -limot->m_bounce* vel;
|
||||
if (newc > info->m_constraintError[srow])
|
||||
info->m_constraintError[srow] = newc;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (vel > 0)
|
||||
{
|
||||
b3Scalar newc = -limot->m_bounce * vel;
|
||||
if (newc < info->m_constraintError[srow])
|
||||
info->m_constraintError[srow] = newc;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
///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 b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
|
||||
{
|
||||
if((axis >= 0) && (axis < 3))
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case B3_CONSTRAINT_STOP_ERP :
|
||||
m_linearLimits.m_stopERP[axis] = value;
|
||||
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case B3_CONSTRAINT_STOP_CFM :
|
||||
m_linearLimits.m_stopCFM[axis] = value;
|
||||
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM :
|
||||
m_linearLimits.m_normalCFM[axis] = value;
|
||||
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
default :
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else if((axis >=3) && (axis < 6))
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case B3_CONSTRAINT_STOP_ERP :
|
||||
m_angularLimits[axis - 3].m_stopERP = value;
|
||||
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case B3_CONSTRAINT_STOP_CFM :
|
||||
m_angularLimits[axis - 3].m_stopCFM = value;
|
||||
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM :
|
||||
m_angularLimits[axis - 3].m_normalCFM = value;
|
||||
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
default :
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
|
||||
///return the local value of parameter
|
||||
b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
|
||||
{
|
||||
b3Scalar retVal = 0;
|
||||
if((axis >= 0) && (axis < 3))
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case B3_CONSTRAINT_STOP_ERP :
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_stopERP[axis];
|
||||
break;
|
||||
case B3_CONSTRAINT_STOP_CFM :
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_stopCFM[axis];
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM :
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_normalCFM[axis];
|
||||
break;
|
||||
default :
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else if((axis >=3) && (axis < 6))
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case B3_CONSTRAINT_STOP_ERP :
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_stopERP;
|
||||
break;
|
||||
case B3_CONSTRAINT_STOP_CFM :
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_stopCFM;
|
||||
break;
|
||||
case B3_CONSTRAINT_CFM :
|
||||
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_normalCFM;
|
||||
break;
|
||||
default :
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
b3AssertConstrParams(0);
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyCL* bodies)
|
||||
{
|
||||
b3Vector3 zAxis = axis1.normalized();
|
||||
b3Vector3 yAxis = axis2.normalized();
|
||||
b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
|
||||
b3Transform frameInW;
|
||||
frameInW.setIdentity();
|
||||
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
|
||||
xAxis[1], yAxis[1], zAxis[1],
|
||||
xAxis[2], yAxis[2], zAxis[2]);
|
||||
|
||||
// now get constraint frame in local coordinate systems
|
||||
m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
|
||||
m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
|
||||
|
||||
calculateTransforms(bodies);
|
||||
}
|
||||
553
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h
Normal file
553
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h
Normal file
@ -0,0 +1,553 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
/// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev
|
||||
/// Added support for generic constraint solver through getInfo1/getInfo2 methods
|
||||
|
||||
/*
|
||||
2007-09-09
|
||||
b3Generic6DofConstraint Refactored by Francisco Le?n
|
||||
email: projectileman@yahoo.com
|
||||
http://gimpact.sf.net
|
||||
*/
|
||||
|
||||
|
||||
#ifndef B3_GENERIC_6DOF_CONSTRAINT_H
|
||||
#define B3_GENERIC_6DOF_CONSTRAINT_H
|
||||
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "b3JacobianEntry.h"
|
||||
#include "b3TypedConstraint.h"
|
||||
|
||||
struct b3RigidBodyCL;
|
||||
|
||||
|
||||
|
||||
|
||||
//! Rotation Limit structure for generic joints
|
||||
class b3RotationalLimitMotor
|
||||
{
|
||||
public:
|
||||
//! limit_parameters
|
||||
//!@{
|
||||
b3Scalar m_loLimit;//!< joint limit
|
||||
b3Scalar m_hiLimit;//!< joint limit
|
||||
b3Scalar m_targetVelocity;//!< target motor velocity
|
||||
b3Scalar m_maxMotorForce;//!< max force on motor
|
||||
b3Scalar m_maxLimitForce;//!< max force on limit
|
||||
b3Scalar m_damping;//!< Damping.
|
||||
b3Scalar m_limitSoftness;//! Relaxation factor
|
||||
b3Scalar m_normalCFM;//!< Constraint force mixing factor
|
||||
b3Scalar m_stopERP;//!< Error tolerance factor when joint is at limit
|
||||
b3Scalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
|
||||
b3Scalar m_bounce;//!< restitution factor
|
||||
bool m_enableMotor;
|
||||
|
||||
//!@}
|
||||
|
||||
//! temp_variables
|
||||
//!@{
|
||||
b3Scalar m_currentLimitError;//! How much is violated this limit
|
||||
b3Scalar m_currentPosition; //! current value of angle
|
||||
int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
|
||||
b3Scalar m_accumulatedImpulse;
|
||||
//!@}
|
||||
|
||||
b3RotationalLimitMotor()
|
||||
{
|
||||
m_accumulatedImpulse = 0.f;
|
||||
m_targetVelocity = 0;
|
||||
m_maxMotorForce = 0.1f;
|
||||
m_maxLimitForce = 300.0f;
|
||||
m_loLimit = 1.0f;
|
||||
m_hiLimit = -1.0f;
|
||||
m_normalCFM = 0.f;
|
||||
m_stopERP = 0.2f;
|
||||
m_stopCFM = 0.f;
|
||||
m_bounce = 0.0f;
|
||||
m_damping = 1.0f;
|
||||
m_limitSoftness = 0.5f;
|
||||
m_currentLimit = 0;
|
||||
m_currentLimitError = 0;
|
||||
m_enableMotor = false;
|
||||
}
|
||||
|
||||
b3RotationalLimitMotor(const b3RotationalLimitMotor & limot)
|
||||
{
|
||||
m_targetVelocity = limot.m_targetVelocity;
|
||||
m_maxMotorForce = limot.m_maxMotorForce;
|
||||
m_limitSoftness = limot.m_limitSoftness;
|
||||
m_loLimit = limot.m_loLimit;
|
||||
m_hiLimit = limot.m_hiLimit;
|
||||
m_normalCFM = limot.m_normalCFM;
|
||||
m_stopERP = limot.m_stopERP;
|
||||
m_stopCFM = limot.m_stopCFM;
|
||||
m_bounce = limot.m_bounce;
|
||||
m_currentLimit = limot.m_currentLimit;
|
||||
m_currentLimitError = limot.m_currentLimitError;
|
||||
m_enableMotor = limot.m_enableMotor;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//! Is limited
|
||||
bool isLimited()
|
||||
{
|
||||
if(m_loLimit > m_hiLimit) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! Need apply correction
|
||||
bool needApplyTorques()
|
||||
{
|
||||
if(m_currentLimit == 0 && m_enableMotor == false) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! calculates error
|
||||
/*!
|
||||
calculates m_currentLimit and m_currentLimitError.
|
||||
*/
|
||||
int testLimitValue(b3Scalar test_value);
|
||||
|
||||
//! apply the correction impulses for two bodies
|
||||
b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyCL * body0, b3RigidBodyCL * body1);
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class b3TranslationalLimitMotor
|
||||
{
|
||||
public:
|
||||
b3Vector3 m_lowerLimit;//!< the constraint lower limits
|
||||
b3Vector3 m_upperLimit;//!< the constraint upper limits
|
||||
b3Vector3 m_accumulatedImpulse;
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
b3Scalar m_limitSoftness;//!< Softness for linear limit
|
||||
b3Scalar m_damping;//!< Damping for linear limit
|
||||
b3Scalar m_restitution;//! Bounce parameter for linear limit
|
||||
b3Vector3 m_normalCFM;//!< Constraint force mixing factor
|
||||
b3Vector3 m_stopERP;//!< Error tolerance factor when joint is at limit
|
||||
b3Vector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
|
||||
//!@}
|
||||
bool m_enableMotor[3];
|
||||
b3Vector3 m_targetVelocity;//!< target motor velocity
|
||||
b3Vector3 m_maxMotorForce;//!< max force on motor
|
||||
b3Vector3 m_currentLimitError;//! How much is violated this limit
|
||||
b3Vector3 m_currentLinearDiff;//! Current relative offset of constraint frames
|
||||
int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
|
||||
|
||||
b3TranslationalLimitMotor()
|
||||
{
|
||||
m_lowerLimit.setValue(0.f,0.f,0.f);
|
||||
m_upperLimit.setValue(0.f,0.f,0.f);
|
||||
m_accumulatedImpulse.setValue(0.f,0.f,0.f);
|
||||
m_normalCFM.setValue(0.f, 0.f, 0.f);
|
||||
m_stopERP.setValue(0.2f, 0.2f, 0.2f);
|
||||
m_stopCFM.setValue(0.f, 0.f, 0.f);
|
||||
|
||||
m_limitSoftness = 0.7f;
|
||||
m_damping = b3Scalar(1.0f);
|
||||
m_restitution = b3Scalar(0.5f);
|
||||
for(int i=0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = false;
|
||||
m_targetVelocity[i] = b3Scalar(0.f);
|
||||
m_maxMotorForce[i] = b3Scalar(0.f);
|
||||
}
|
||||
}
|
||||
|
||||
b3TranslationalLimitMotor(const b3TranslationalLimitMotor & other )
|
||||
{
|
||||
m_lowerLimit = other.m_lowerLimit;
|
||||
m_upperLimit = other.m_upperLimit;
|
||||
m_accumulatedImpulse = other.m_accumulatedImpulse;
|
||||
|
||||
m_limitSoftness = other.m_limitSoftness ;
|
||||
m_damping = other.m_damping;
|
||||
m_restitution = other.m_restitution;
|
||||
m_normalCFM = other.m_normalCFM;
|
||||
m_stopERP = other.m_stopERP;
|
||||
m_stopCFM = other.m_stopCFM;
|
||||
|
||||
for(int i=0; i < 3; i++)
|
||||
{
|
||||
m_enableMotor[i] = other.m_enableMotor[i];
|
||||
m_targetVelocity[i] = other.m_targetVelocity[i];
|
||||
m_maxMotorForce[i] = other.m_maxMotorForce[i];
|
||||
}
|
||||
}
|
||||
|
||||
//! Test limit
|
||||
/*!
|
||||
- free means upper < lower,
|
||||
- locked means upper == lower
|
||||
- limited means upper > lower
|
||||
- limitIndex: first 3 are linear, next 3 are angular
|
||||
*/
|
||||
inline bool isLimited(int limitIndex)
|
||||
{
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
inline bool needApplyForce(int limitIndex)
|
||||
{
|
||||
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
|
||||
return true;
|
||||
}
|
||||
int testLimitValue(int limitIndex, b3Scalar test_value);
|
||||
|
||||
|
||||
b3Scalar solveLinearAxis(
|
||||
b3Scalar timeStep,
|
||||
b3Scalar jacDiagABInv,
|
||||
b3RigidBodyCL& body1,const b3Vector3 &pointInA,
|
||||
b3RigidBodyCL& body2,const b3Vector3 &pointInB,
|
||||
int limit_index,
|
||||
const b3Vector3 & axis_normal_on_a,
|
||||
const b3Vector3 & anchorPos);
|
||||
|
||||
|
||||
};
|
||||
|
||||
enum b36DofFlags
|
||||
{
|
||||
B3_6DOF_FLAGS_CFM_NORM = 1,
|
||||
B3_6DOF_FLAGS_CFM_STOP = 2,
|
||||
B3_6DOF_FLAGS_ERP_STOP = 4
|
||||
};
|
||||
#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
|
||||
|
||||
|
||||
/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/*!
|
||||
b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
|
||||
currently this limit supports rotational motors<br>
|
||||
<ul>
|
||||
<li> For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method.
|
||||
At this moment translational motors are not supported. May be in the future. </li>
|
||||
|
||||
<li> For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit.
|
||||
This is accessible through b3Generic6DofConstraint.getLimitMotor method,
|
||||
This brings support for limit parameters and motors. </li>
|
||||
|
||||
<li> Angulars limits have these possible ranges:
|
||||
<table border=1 >
|
||||
<tr>
|
||||
<td><b>AXIS</b></td>
|
||||
<td><b>MIN ANGLE</b></td>
|
||||
<td><b>MAX ANGLE</b></td>
|
||||
</tr><tr>
|
||||
<td>X</td>
|
||||
<td>-PI</td>
|
||||
<td>PI</td>
|
||||
</tr><tr>
|
||||
<td>Y</td>
|
||||
<td>-PI/2</td>
|
||||
<td>PI/2</td>
|
||||
</tr><tr>
|
||||
<td>Z</td>
|
||||
<td>-PI</td>
|
||||
<td>PI</td>
|
||||
</tr>
|
||||
</table>
|
||||
</li>
|
||||
</ul>
|
||||
|
||||
*/
|
||||
B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint
|
||||
{
|
||||
protected:
|
||||
|
||||
//! relative_frames
|
||||
//!@{
|
||||
b3Transform m_frameInA;//!< the constraint space w.r.t body A
|
||||
b3Transform m_frameInB;//!< the constraint space w.r.t body B
|
||||
//!@}
|
||||
|
||||
//! Jacobians
|
||||
//!@{
|
||||
b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
|
||||
b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
|
||||
//!@}
|
||||
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
b3TranslationalLimitMotor m_linearLimits;
|
||||
//!@}
|
||||
|
||||
|
||||
//! hinge_parameters
|
||||
//!@{
|
||||
b3RotationalLimitMotor m_angularLimits[3];
|
||||
//!@}
|
||||
|
||||
|
||||
protected:
|
||||
//! temporal variables
|
||||
//!@{
|
||||
b3Scalar m_timeStep;
|
||||
b3Transform m_calculatedTransformA;
|
||||
b3Transform m_calculatedTransformB;
|
||||
b3Vector3 m_calculatedAxisAngleDiff;
|
||||
b3Vector3 m_calculatedAxis[3];
|
||||
b3Vector3 m_calculatedLinearDiff;
|
||||
b3Scalar m_factA;
|
||||
b3Scalar m_factB;
|
||||
bool m_hasStaticBody;
|
||||
|
||||
b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
|
||||
|
||||
bool m_useLinearReferenceFrameA;
|
||||
bool m_useOffsetForConstraintFrame;
|
||||
|
||||
int m_flags;
|
||||
|
||||
//!@}
|
||||
|
||||
b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
|
||||
{
|
||||
b3Assert(0);
|
||||
(void) other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
int setAngularLimits(b3ConstraintInfo2 *info, int row_offset,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
|
||||
|
||||
int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
|
||||
|
||||
|
||||
// tests linear limits
|
||||
void calculateLinearInfo();
|
||||
|
||||
//! calcs the euler angles between the two bodies.
|
||||
void calculateAngleInfo();
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
bool m_useSolveConstraintObsolete;
|
||||
|
||||
b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyCL* bodies);
|
||||
|
||||
//! Calcs global transform of the offsets
|
||||
/*!
|
||||
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
|
||||
\sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
|
||||
*/
|
||||
void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyCL* bodies);
|
||||
|
||||
void calculateTransforms(const b3RigidBodyCL* bodies);
|
||||
|
||||
//! Gets the global transform of the offset for body A
|
||||
/*!
|
||||
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
|
||||
*/
|
||||
const b3Transform & getCalculatedTransformA() const
|
||||
{
|
||||
return m_calculatedTransformA;
|
||||
}
|
||||
|
||||
//! Gets the global transform of the offset for body B
|
||||
/*!
|
||||
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
|
||||
*/
|
||||
const b3Transform & getCalculatedTransformB() const
|
||||
{
|
||||
return m_calculatedTransformB;
|
||||
}
|
||||
|
||||
const b3Transform & getFrameOffsetA() const
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
const b3Transform & getFrameOffsetB() const
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
|
||||
b3Transform & getFrameOffsetA()
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
b3Transform & getFrameOffsetB()
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
|
||||
|
||||
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
|
||||
|
||||
void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
|
||||
|
||||
virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyCL* bodies);
|
||||
|
||||
void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyCL* bodies);
|
||||
|
||||
|
||||
void updateRHS(b3Scalar timeStep);
|
||||
|
||||
//! Get the rotation axis in global coordinates
|
||||
b3Vector3 getAxis(int axis_index) const;
|
||||
|
||||
//! Get the relative Euler angle
|
||||
/*!
|
||||
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
b3Scalar getAngle(int axis_index) const;
|
||||
|
||||
//! Get the relative position of the constraint pivot
|
||||
/*!
|
||||
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
b3Scalar getRelativePivotPosition(int axis_index) const;
|
||||
|
||||
void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyCL* bodies);
|
||||
|
||||
//! Test angular limit.
|
||||
/*!
|
||||
Calculates angular correction and returns true if limit needs to be corrected.
|
||||
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
|
||||
*/
|
||||
bool testAngularLimitMotor(int axis_index);
|
||||
|
||||
void setLinearLowerLimit(const b3Vector3& linearLower)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit = linearLower;
|
||||
}
|
||||
|
||||
void getLinearLowerLimit(b3Vector3& linearLower)
|
||||
{
|
||||
linearLower = m_linearLimits.m_lowerLimit;
|
||||
}
|
||||
|
||||
void setLinearUpperLimit(const b3Vector3& linearUpper)
|
||||
{
|
||||
m_linearLimits.m_upperLimit = linearUpper;
|
||||
}
|
||||
|
||||
void getLinearUpperLimit(b3Vector3& linearUpper)
|
||||
{
|
||||
linearUpper = m_linearLimits.m_upperLimit;
|
||||
}
|
||||
|
||||
void setAngularLowerLimit(const b3Vector3& angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
|
||||
}
|
||||
|
||||
void getAngularLowerLimit(b3Vector3& angularLower)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
angularLower[i] = m_angularLimits[i].m_loLimit;
|
||||
}
|
||||
|
||||
void setAngularUpperLimit(const b3Vector3& angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
|
||||
}
|
||||
|
||||
void getAngularUpperLimit(b3Vector3& angularUpper)
|
||||
{
|
||||
for(int i = 0; i < 3; i++)
|
||||
angularUpper[i] = m_angularLimits[i].m_hiLimit;
|
||||
}
|
||||
|
||||
//! Retrieves the angular limit informacion
|
||||
b3RotationalLimitMotor * getRotationalLimitMotor(int index)
|
||||
{
|
||||
return &m_angularLimits[index];
|
||||
}
|
||||
|
||||
//! Retrieves the limit informacion
|
||||
b3TranslationalLimitMotor * getTranslationalLimitMotor()
|
||||
{
|
||||
return &m_linearLimits;
|
||||
}
|
||||
|
||||
//first 3 are linear, next 3 are angular
|
||||
void setLimit(int axis, b3Scalar lo, b3Scalar hi)
|
||||
{
|
||||
if(axis<3)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit[axis] = lo;
|
||||
m_linearLimits.m_upperLimit[axis] = hi;
|
||||
}
|
||||
else
|
||||
{
|
||||
lo = b3NormalizeAngle(lo);
|
||||
hi = b3NormalizeAngle(hi);
|
||||
m_angularLimits[axis-3].m_loLimit = lo;
|
||||
m_angularLimits[axis-3].m_hiLimit = hi;
|
||||
}
|
||||
}
|
||||
|
||||
//! Test limit
|
||||
/*!
|
||||
- free means upper < lower,
|
||||
- locked means upper == lower
|
||||
- limited means upper > lower
|
||||
- limitIndex: first 3 are linear, next 3 are angular
|
||||
*/
|
||||
bool isLimited(int limitIndex)
|
||||
{
|
||||
if(limitIndex<3)
|
||||
{
|
||||
return m_linearLimits.isLimited(limitIndex);
|
||||
|
||||
}
|
||||
return m_angularLimits[limitIndex-3].isLimited();
|
||||
}
|
||||
|
||||
virtual void calcAnchorPos(const b3RigidBodyCL* bodies); // overridable
|
||||
|
||||
int get_limit_motor_info2( b3RotationalLimitMotor * limot,
|
||||
const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
|
||||
b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
|
||||
|
||||
// access for UseFrameOffset
|
||||
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
|
||||
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
|
||||
|
||||
///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.
|
||||
virtual void setParam(int num, b3Scalar value, int axis = -1);
|
||||
///return the local value of parameter
|
||||
virtual b3Scalar getParam(int num, int axis = -1) const;
|
||||
|
||||
void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyCL* bodies);
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //B3_GENERIC_6DOF_CONSTRAINT_H
|
||||
155
src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
Normal file
155
src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
Normal file
@ -0,0 +1,155 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef B3_JACOBIAN_ENTRY_H
|
||||
#define B3_JACOBIAN_ENTRY_H
|
||||
|
||||
#include "Bullet3Common/b3Matrix3x3.h"
|
||||
|
||||
|
||||
//notes:
|
||||
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
|
||||
// which makes the b3JacobianEntry memory layout 16 bytes
|
||||
// if you only are interested in angular part, just feed massInvA and massInvB zero
|
||||
|
||||
/// Jacobian entry is an abstraction that allows to describe constraints
|
||||
/// it can be used in combination with a constraint solver
|
||||
/// Can be used to relate the effect of an impulse to the constraint error
|
||||
B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry
|
||||
{
|
||||
public:
|
||||
b3JacobianEntry() {};
|
||||
//constraint between two different rigidbodies
|
||||
b3JacobianEntry(
|
||||
const b3Matrix3x3& world2A,
|
||||
const b3Matrix3x3& world2B,
|
||||
const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
|
||||
const b3Vector3& jointAxis,
|
||||
const b3Vector3& inertiaInvA,
|
||||
const b3Scalar massInvA,
|
||||
const b3Vector3& inertiaInvB,
|
||||
const b3Scalar massInvB)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
|
||||
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
b3Assert(m_Adiag > b3Scalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
b3JacobianEntry(const b3Vector3& jointAxis,
|
||||
const b3Matrix3x3& world2A,
|
||||
const b3Matrix3x3& world2B,
|
||||
const b3Vector3& inertiaInvA,
|
||||
const b3Vector3& inertiaInvB)
|
||||
:m_linearJointAxis(b3Vector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
|
||||
{
|
||||
m_aJ= world2A*jointAxis;
|
||||
m_bJ = world2B*-jointAxis;
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
b3Assert(m_Adiag > b3Scalar(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
b3JacobianEntry(const b3Vector3& axisInA,
|
||||
const b3Vector3& axisInB,
|
||||
const b3Vector3& inertiaInvA,
|
||||
const b3Vector3& inertiaInvB)
|
||||
: m_linearJointAxis(b3Vector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
|
||||
, m_aJ(axisInA)
|
||||
, m_bJ(-axisInB)
|
||||
{
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
b3Assert(m_Adiag > b3Scalar(0.0));
|
||||
}
|
||||
|
||||
//constraint on one rigidbody
|
||||
b3JacobianEntry(
|
||||
const b3Matrix3x3& world2A,
|
||||
const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
|
||||
const b3Vector3& jointAxis,
|
||||
const b3Vector3& inertiaInvA,
|
||||
const b3Scalar massInvA)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ= world2A*(rel_pos1.cross(jointAxis));
|
||||
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = b3Vector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
|
||||
|
||||
b3Assert(m_Adiag > b3Scalar(0.0));
|
||||
}
|
||||
|
||||
b3Scalar getDiagonal() const { return m_Adiag; }
|
||||
|
||||
// for two constraints on the same rigidbody (for example vehicle friction)
|
||||
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
|
||||
{
|
||||
const b3JacobianEntry& jacA = *this;
|
||||
b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
|
||||
b3Scalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
|
||||
return lin + ang;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
|
||||
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB,const b3Scalar massInvA,const b3Scalar massInvB) const
|
||||
{
|
||||
const b3JacobianEntry& jacA = *this;
|
||||
b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
|
||||
b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
|
||||
b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
|
||||
b3Vector3 lin0 = massInvA * lin ;
|
||||
b3Vector3 lin1 = massInvB * lin;
|
||||
b3Vector3 sum = ang0+ang1+lin0+lin1;
|
||||
return sum[0]+sum[1]+sum[2];
|
||||
}
|
||||
|
||||
b3Scalar getRelativeVelocity(const b3Vector3& linvelA,const b3Vector3& angvelA,const b3Vector3& linvelB,const b3Vector3& angvelB)
|
||||
{
|
||||
b3Vector3 linrel = linvelA - linvelB;
|
||||
b3Vector3 angvela = angvelA * m_aJ;
|
||||
b3Vector3 angvelb = angvelB * m_bJ;
|
||||
linrel *= m_linearJointAxis;
|
||||
angvela += angvelb;
|
||||
angvela += linrel;
|
||||
b3Scalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
|
||||
return rel_vel2 + B3_EPSILON;
|
||||
}
|
||||
//private:
|
||||
|
||||
b3Vector3 m_linearJointAxis;
|
||||
b3Vector3 m_aJ;
|
||||
b3Vector3 m_bJ;
|
||||
b3Vector3 m_0MinvJt;
|
||||
b3Vector3 m_1MinvJt;
|
||||
//Optimization: can be stored in the w/last component of one of the vectors
|
||||
b3Scalar m_Adiag;
|
||||
|
||||
};
|
||||
|
||||
#endif //B3_JACOBIAN_ENTRY_H
|
||||
@ -1137,7 +1137,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
b3TypedConstraint* constraint = constraints[j];
|
||||
constraint->buildJacobian();
|
||||
|
||||
constraint->internalSetAppliedImpulse(0.0f);
|
||||
}
|
||||
}
|
||||
@ -1169,7 +1169,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
|
||||
}
|
||||
if (constraints[i]->isEnabled())
|
||||
{
|
||||
constraints[i]->getInfo1(&info1);
|
||||
constraints[i]->getInfo1(&info1,bodies);
|
||||
} else
|
||||
{
|
||||
info1.m_numConstraintRows = 0;
|
||||
|
||||
@ -41,22 +41,13 @@ m_useSolveConstraintObsolete(false)
|
||||
}
|
||||
*/
|
||||
|
||||
void b3Point2PointConstraint::buildJacobian()
|
||||
|
||||
void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
|
||||
{
|
||||
|
||||
///we need it for both methods
|
||||
{
|
||||
m_appliedImpulse = b3Scalar(0.);
|
||||
}
|
||||
|
||||
getInfo1NonVirtual(info,bodies);
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info)
|
||||
{
|
||||
getInfo1NonVirtual(info);
|
||||
}
|
||||
|
||||
void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info)
|
||||
void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
|
||||
@ -78,11 +78,10 @@ public:
|
||||
//b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA);
|
||||
|
||||
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void getInfo1 (b3ConstraintInfo1* info);
|
||||
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
|
||||
|
||||
void getInfo1NonVirtual (b3ConstraintInfo1* info);
|
||||
void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
|
||||
|
||||
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies);
|
||||
|
||||
|
||||
@ -157,8 +157,6 @@ public:
|
||||
m_overrideNumSolverIterations = overideNumIterations;
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void buildJacobian() {};
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
|
||||
@ -170,7 +168,7 @@ public:
|
||||
}
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo1 (b3ConstraintInfo1* info)=0;
|
||||
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)=0;
|
||||
|
||||
///internal method used by the constraint solver, don't use them directly
|
||||
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies)=0;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user