mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
Add a virtual createConstraintRows method, to easier experiment with different kinds of btMultiBodyConstraint
This commit is contained in:
erwin.coumans
parent
c2bece5280
commit
75f17509cc
@ -209,156 +209,157 @@ void FeatherstoneMultiBodyDemo::initPhysics()
|
||||
}
|
||||
|
||||
|
||||
createFeatherstoneMultiBody(world, 5, btVector3 (20,29.5,-2), true, true);
|
||||
btMultiBody* mb = createFeatherstoneMultiBody(world, 1, btVector3 (20,29.5,-2), true, true,true);
|
||||
|
||||
createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false);
|
||||
|
||||
mb = createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false,true);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic)
|
||||
btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep)
|
||||
{
|
||||
{
|
||||
int n_links = numLinks;
|
||||
float mass = 13.5;
|
||||
btVector3 inertia(91,344,253);
|
||||
bool canSleep = true;//false;
|
||||
|
||||
btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep);
|
||||
int n_links = numLinks;
|
||||
float mass = 13.5;
|
||||
btVector3 inertia(91,344,253);
|
||||
|
||||
|
||||
btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep);
|
||||
|
||||
//btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1);
|
||||
btQuaternion orn(0,0,0,1);
|
||||
bod->setBasePos(basePosition);
|
||||
bod->setWorldToBaseRot(orn);
|
||||
btVector3 vel(0,0,0);
|
||||
bod->setBaseVel(vel);
|
||||
//btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1);
|
||||
btQuaternion orn(0,0,0,1);
|
||||
bod->setBasePos(basePosition);
|
||||
bod->setWorldToBaseRot(orn);
|
||||
btVector3 vel(0,0,0);
|
||||
bod->setBaseVel(vel);
|
||||
|
||||
{
|
||||
{
|
||||
|
||||
btVector3 joint_axis_hinge(1,0,0);
|
||||
btVector3 joint_axis_prismatic(0,0,1);
|
||||
btQuaternion parent_to_child = orn.inverse();
|
||||
btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic);
|
||||
btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge);
|
||||
btVector3 joint_axis_hinge(1,0,0);
|
||||
btVector3 joint_axis_prismatic(0,0,1);
|
||||
btQuaternion parent_to_child = orn.inverse();
|
||||
btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic);
|
||||
btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge);
|
||||
|
||||
int this_link_num = -1;
|
||||
int link_num_counter = 0;
|
||||
int this_link_num = -1;
|
||||
int link_num_counter = 0;
|
||||
|
||||
|
||||
|
||||
btVector3 pos(0,0,9.0500002);
|
||||
btVector3 pos(0,0,9.0500002);
|
||||
|
||||
btVector3 joint_axis_position(0,0,4.5250001);
|
||||
btVector3 joint_axis_position(0,0,4.5250001);
|
||||
|
||||
for (int i=0;i<n_links;i++)
|
||||
{
|
||||
float initial_joint_angle=0.3;
|
||||
if (i>0)
|
||||
initial_joint_angle = -0.06f;
|
||||
|
||||
const int child_link_num = link_num_counter++;
|
||||
|
||||
if (usePrismatic && i==(n_links-1))
|
||||
{
|
||||
bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
|
||||
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos));
|
||||
|
||||
} else
|
||||
{
|
||||
bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge,
|
||||
joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)));
|
||||
}
|
||||
bod->setJointPos(child_link_num, initial_joint_angle);
|
||||
this_link_num = i;
|
||||
}
|
||||
|
||||
//add some constraint limit
|
||||
if (usePrismatic)
|
||||
{
|
||||
btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
|
||||
world->addMultiBodyConstraint(limit);
|
||||
}
|
||||
}
|
||||
|
||||
//add a collider for the base
|
||||
for (int i=0;i<n_links;i++)
|
||||
{
|
||||
|
||||
btAlignedObjectArray<btQuaternion> world_to_local;
|
||||
world_to_local.resize(n_links+1);
|
||||
float initial_joint_angle=0.3;
|
||||
if (i>0)
|
||||
initial_joint_angle = -0.06f;
|
||||
|
||||
btAlignedObjectArray<btVector3> local_origin;
|
||||
local_origin.resize(n_links+1);
|
||||
world_to_local[0] = bod->getWorldToBaseRot();
|
||||
local_origin[0] = bod->getBasePos();
|
||||
//float halfExtents[3]={7.5,0.05,4.5};
|
||||
float halfExtents[3]={7.5,0.45,4.5};
|
||||
const int child_link_num = link_num_counter++;
|
||||
|
||||
if (usePrismatic && i==(n_links-1))
|
||||
{
|
||||
|
||||
float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
|
||||
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
|
||||
bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
|
||||
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos));
|
||||
|
||||
|
||||
if (1)
|
||||
{
|
||||
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
|
||||
btRigidBody* body = new btRigidBody(mass,0,box,inertia);
|
||||
btMultiBodyLinkCollider* multiBody= new btMultiBodyLinkCollider(bod,-1);
|
||||
|
||||
body->setCollisionShape(box);
|
||||
multiBody->setCollisionShape(box);
|
||||
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(local_origin[0]);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
body->setWorldTransform(tr);
|
||||
multiBody->setWorldTransform(tr);
|
||||
|
||||
world->addCollisionObject(multiBody, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);
|
||||
multiBody->setFriction(1);
|
||||
bod->addLinkCollider(multiBody);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<bod->getNumLinks();i++)
|
||||
} else
|
||||
{
|
||||
const int parent = bod->getParent(i);
|
||||
world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1];
|
||||
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i)));
|
||||
bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge,
|
||||
joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)));
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<bod->getNumLinks();i++)
|
||||
{
|
||||
|
||||
btVector3 posr = local_origin[i+1];
|
||||
float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
||||
|
||||
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
|
||||
|
||||
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
|
||||
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i);
|
||||
|
||||
col->setCollisionShape(box);
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(posr);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
col->setWorldTransform(tr);
|
||||
col->setFriction(1);
|
||||
world->addCollisionObject(col,btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);//,2,1);
|
||||
|
||||
bod->addLinkCollider(col);
|
||||
//app->drawBox(halfExtents, pos,quat);
|
||||
}
|
||||
|
||||
bod->setJointPos(child_link_num, initial_joint_angle);
|
||||
this_link_num = i;
|
||||
}
|
||||
|
||||
//add some constraint limit
|
||||
if (usePrismatic)
|
||||
{
|
||||
btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
|
||||
world->addMultiBodyConstraint(limit);
|
||||
}
|
||||
world->addMultiBody(bod);
|
||||
}
|
||||
|
||||
//add a collider for the base
|
||||
{
|
||||
|
||||
btAlignedObjectArray<btQuaternion> world_to_local;
|
||||
world_to_local.resize(n_links+1);
|
||||
|
||||
btAlignedObjectArray<btVector3> local_origin;
|
||||
local_origin.resize(n_links+1);
|
||||
world_to_local[0] = bod->getWorldToBaseRot();
|
||||
local_origin[0] = bod->getBasePos();
|
||||
//float halfExtents[3]={7.5,0.05,4.5};
|
||||
float halfExtents[3]={7.5,0.45,4.5};
|
||||
{
|
||||
|
||||
float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
|
||||
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
|
||||
|
||||
|
||||
if (1)
|
||||
{
|
||||
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
|
||||
btRigidBody* body = new btRigidBody(mass,0,box,inertia);
|
||||
btMultiBodyLinkCollider* multiBody= new btMultiBodyLinkCollider(bod,-1);
|
||||
|
||||
body->setCollisionShape(box);
|
||||
multiBody->setCollisionShape(box);
|
||||
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(local_origin[0]);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
body->setWorldTransform(tr);
|
||||
multiBody->setWorldTransform(tr);
|
||||
|
||||
world->addCollisionObject(multiBody, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);
|
||||
multiBody->setFriction(1);
|
||||
bod->addLinkCollider(multiBody);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<bod->getNumLinks();i++)
|
||||
{
|
||||
const int parent = bod->getParent(i);
|
||||
world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1];
|
||||
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i)));
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<bod->getNumLinks();i++)
|
||||
{
|
||||
|
||||
btVector3 posr = local_origin[i+1];
|
||||
float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
||||
|
||||
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
|
||||
|
||||
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
|
||||
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i);
|
||||
|
||||
col->setCollisionShape(box);
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(posr);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
col->setWorldTransform(tr);
|
||||
col->setFriction(1);
|
||||
world->addCollisionObject(col,btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);//,2,1);
|
||||
|
||||
bod->addLinkCollider(col);
|
||||
//app->drawBox(halfExtents, pos,quat);
|
||||
}
|
||||
|
||||
}
|
||||
world->addMultiBody(bod);
|
||||
|
||||
return bod;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -28,6 +28,7 @@ subject to the following restrictions:
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
class btMultiBody;
|
||||
class btBroadphaseInterface;
|
||||
class btCollisionShape;
|
||||
class btOverlappingPairCache;
|
||||
@ -53,7 +54,7 @@ class FeatherstoneMultiBodyDemo : public PlatformDemoApplication
|
||||
btDefaultCollisionConfiguration* m_collisionConfiguration;
|
||||
|
||||
|
||||
void createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic);
|
||||
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep);
|
||||
|
||||
public:
|
||||
|
||||
|
||||
@ -21,6 +21,20 @@ subject to the following restrictions:
|
||||
#include "btMultiBody.h"
|
||||
|
||||
class btMultiBody;
|
||||
struct btSolverInfo;
|
||||
|
||||
#include "btMultiBodySolverConstraint.h"
|
||||
|
||||
struct btMultiBodyJacobianData
|
||||
{
|
||||
btAlignedObjectArray<btScalar> m_jacobians;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocities;
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
btAlignedObjectArray<btMatrix3x3> scratch_m;
|
||||
};
|
||||
|
||||
|
||||
class btMultiBodyConstraint
|
||||
{
|
||||
@ -62,7 +76,9 @@ public:
|
||||
virtual int getIslandIdA() const =0;
|
||||
virtual int getIslandIdB() const =0;
|
||||
|
||||
virtual void update()=0;
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)=0;
|
||||
|
||||
int getNumRows() const
|
||||
{
|
||||
|
||||
@ -19,6 +19,7 @@ subject to the following restrictions:
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
|
||||
#include "btMultiBodyConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
|
||||
@ -33,7 +34,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
{
|
||||
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
|
||||
//if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
resolveSingleConstraintRowGenericMultiBody(constraint);
|
||||
//resolveSingleConstraintRowGenericMultiBody(constraint);
|
||||
resolveSingleConstraintRowGeneric(constraint);
|
||||
}
|
||||
|
||||
//solve featherstone normal contact
|
||||
@ -69,9 +71,9 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
|
||||
m_multiBodyNonContactConstraints.resize(0);
|
||||
m_multiBodyNormalContactConstraints.resize(0);
|
||||
m_multiBodyFrictionContactConstraints.resize(0);
|
||||
m_jacobians.resize(0);
|
||||
m_deltaVelocitiesUnitImpulse.resize(0);
|
||||
m_deltaVelocities.resize(0);
|
||||
m_data.m_jacobians.resize(0);
|
||||
m_data.m_deltaVelocitiesUnitImpulse.resize(0);
|
||||
m_data.m_deltaVelocities.resize(0);
|
||||
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
@ -90,7 +92,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
|
||||
void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
|
||||
{
|
||||
for (int i = 0; i < ndof; ++i)
|
||||
m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
|
||||
m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
|
||||
}
|
||||
|
||||
void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
|
||||
@ -108,7 +110,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
|
||||
{
|
||||
ndofA = c.m_multiBodyA->getNumLinks() + 6;
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i];
|
||||
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
|
||||
} else
|
||||
{
|
||||
bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
|
||||
@ -119,7 +121,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
|
||||
{
|
||||
ndofB = c.m_multiBodyB->getNumLinks() + 6;
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i];
|
||||
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
|
||||
} else
|
||||
{
|
||||
bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
|
||||
@ -148,8 +150,8 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
|
||||
|
||||
if (c.m_multiBodyA)
|
||||
{
|
||||
applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
|
||||
c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
|
||||
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||
} else
|
||||
{
|
||||
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
@ -157,8 +159,8 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
|
||||
}
|
||||
if (c.m_multiBodyB)
|
||||
{
|
||||
applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
|
||||
c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
|
||||
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||
} else
|
||||
{
|
||||
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
@ -180,14 +182,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
|
||||
{
|
||||
ndofA = c.m_multiBodyA->getNumLinks() + 6;
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i];
|
||||
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
|
||||
}
|
||||
|
||||
if (c.m_multiBodyB)
|
||||
{
|
||||
ndofB = c.m_multiBodyB->getNumLinks() + 6;
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i];
|
||||
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
|
||||
}
|
||||
|
||||
|
||||
@ -212,13 +214,13 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
|
||||
|
||||
if (c.m_multiBodyA)
|
||||
{
|
||||
applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
|
||||
c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
|
||||
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||
}
|
||||
if (c.m_multiBodyB)
|
||||
{
|
||||
applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
|
||||
c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
|
||||
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||
}
|
||||
}
|
||||
|
||||
@ -261,23 +263,23 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
|
||||
if (solverConstraint.m_deltaVelAindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelAindex = m_deltaVelocities.size();
|
||||
solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
|
||||
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
|
||||
m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA);
|
||||
m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
|
||||
} else
|
||||
{
|
||||
btAssert(m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
|
||||
btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
solverConstraint.m_jacAindex = m_jacobians.size();
|
||||
m_jacobians.resize(m_jacobians.size()+ndofA);
|
||||
m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
||||
btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
|
||||
solverConstraint.m_jacAindex = m_data.m_jacobians.size();
|
||||
m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
|
||||
m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
||||
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
|
||||
|
||||
float* jac1=&m_jacobians[solverConstraint.m_jacAindex];
|
||||
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, scratch_r, scratch_v, scratch_m);
|
||||
float* delta = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacAindex],delta,scratch_r, scratch_v);
|
||||
float* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
|
||||
float* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
|
||||
} else
|
||||
{
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
|
||||
@ -293,19 +295,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
if (solverConstraint.m_deltaVelBindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelBindex = m_deltaVelocities.size();
|
||||
solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
|
||||
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
|
||||
m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB);
|
||||
m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
solverConstraint.m_jacBindex = m_jacobians.size();
|
||||
solverConstraint.m_jacBindex = m_data.m_jacobians.size();
|
||||
|
||||
m_jacobians.resize(m_jacobians.size()+ndofB);
|
||||
m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
|
||||
m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
|
||||
m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
|
||||
|
||||
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_jacobians[solverConstraint.m_jacBindex], scratch_r, scratch_v, scratch_m);
|
||||
multiBodyB->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacBindex],&m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],scratch_r, scratch_v);
|
||||
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
|
||||
multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
|
||||
} else
|
||||
{
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
|
||||
@ -327,8 +329,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumLinks() + 6;
|
||||
jacA = &m_jacobians[solverConstraint.m_jacAindex];
|
||||
lambdaA = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
float j = jacA[i] ;
|
||||
@ -346,8 +348,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
jacB = &m_jacobians[solverConstraint.m_jacBindex];
|
||||
lambdaB = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
|
||||
lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
float j = jacB[i] ;
|
||||
@ -403,7 +405,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumLinks() + 6;
|
||||
btScalar* jacA = &m_jacobians[solverConstraint.m_jacAindex];
|
||||
btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA ; ++i)
|
||||
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
|
||||
} else
|
||||
@ -416,7 +418,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
if (multiBodyB)
|
||||
{
|
||||
ndofB = multiBodyB->getNumLinks() + 6;
|
||||
btScalar* jacB = &m_jacobians[solverConstraint.m_jacBindex];
|
||||
btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB ; ++i)
|
||||
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
||||
|
||||
@ -449,7 +451,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
if (multiBodyA)
|
||||
{
|
||||
btScalar impulse = solverConstraint.m_appliedImpulse;
|
||||
btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
|
||||
} else
|
||||
@ -460,7 +462,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
if (multiBodyB)
|
||||
{
|
||||
btScalar impulse = solverConstraint.m_appliedImpulse;
|
||||
btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
multiBodyB->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
|
||||
} else
|
||||
@ -524,257 +526,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
|
||||
|
||||
|
||||
void btMultiBodyConstraintSolver::setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
|
||||
btScalar* jacOrgA,btScalar* jacOrgB,
|
||||
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
|
||||
BT_PROFILE("setupMultiBodyContactConstraint");
|
||||
|
||||
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
|
||||
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
|
||||
const int ndofA = multiBodyA->getNumLinks() + 6;
|
||||
|
||||
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
|
||||
if (constraintRow.m_deltaVelAindex <0)
|
||||
{
|
||||
constraintRow.m_deltaVelAindex = m_deltaVelocities.size();
|
||||
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
|
||||
m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA);
|
||||
} else
|
||||
{
|
||||
btAssert(m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
constraintRow.m_jacAindex = m_jacobians.size();
|
||||
m_jacobians.resize(m_jacobians.size()+ndofA);
|
||||
m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
||||
btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
|
||||
for (int i=0;i<ndofA;i++)
|
||||
m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
|
||||
|
||||
float* delta = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
||||
multiBodyA->calcAccelerationDeltas(&m_jacobians[constraintRow.m_jacAindex],delta,scratch_r, scratch_v);
|
||||
}
|
||||
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
|
||||
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
if (constraintRow.m_deltaVelBindex <0)
|
||||
{
|
||||
constraintRow.m_deltaVelBindex = m_deltaVelocities.size();
|
||||
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
|
||||
m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
constraintRow.m_jacBindex = m_jacobians.size();
|
||||
m_jacobians.resize(m_jacobians.size()+ndofB);
|
||||
|
||||
for (int i=0;i<ndofB;i++)
|
||||
m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
|
||||
|
||||
m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
|
||||
multiBodyB->calcAccelerationDeltas(&m_jacobians[constraintRow.m_jacBindex],&m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],scratch_r, scratch_v);
|
||||
}
|
||||
{
|
||||
|
||||
btVector3 vec;
|
||||
btScalar denom0 = 0.f;
|
||||
btScalar denom1 = 0.f;
|
||||
btScalar* jacB = 0;
|
||||
btScalar* jacA = 0;
|
||||
btScalar* lambdaA =0;
|
||||
btScalar* lambdaB =0;
|
||||
int ndofA = 0;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumLinks() + 6;
|
||||
jacA = &m_jacobians[constraintRow.m_jacAindex];
|
||||
lambdaA = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
float j = jacA[i] ;
|
||||
float l =lambdaA[i];
|
||||
denom0 += j*l;
|
||||
}
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
jacB = &m_jacobians[constraintRow.m_jacBindex];
|
||||
lambdaB = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
float j = jacB[i] ;
|
||||
float l =lambdaB[i];
|
||||
denom1 += j*l;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (multiBodyA && (multiBodyA==multiBodyB))
|
||||
{
|
||||
// ndof1 == ndof2 in this case
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
denom1 += jacB[i] * lambdaA[i];
|
||||
denom1 += jacA[i] * lambdaB[i];
|
||||
}
|
||||
}
|
||||
|
||||
float d = denom0+denom1;
|
||||
if (btFabs(d)>SIMD_EPSILON)
|
||||
{
|
||||
|
||||
constraintRow.m_jacDiagABInv = 1.f/(d);
|
||||
} else
|
||||
{
|
||||
constraintRow.m_jacDiagABInv = 1.f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
//compute rhs and remaining constraintRow fields
|
||||
|
||||
|
||||
|
||||
btScalar restitution = 0.f;
|
||||
|
||||
|
||||
btScalar rel_vel = 0.f;
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
{
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumLinks() + 6;
|
||||
btScalar* jacA = &m_jacobians[constraintRow.m_jacAindex];
|
||||
for (int i = 0; i < ndofA ; ++i)
|
||||
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
ndofB = multiBodyB->getNumLinks() + 6;
|
||||
btScalar* jacB = &m_jacobians[constraintRow.m_jacBindex];
|
||||
for (int i = 0; i < ndofB ; ++i)
|
||||
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
||||
|
||||
}
|
||||
|
||||
constraintRow.m_friction = combinedFrictionCoeff;
|
||||
|
||||
|
||||
restitution = restitutionCurve(rel_vel, combinedRestitutionCoeff);
|
||||
if (restitution <= btScalar(0.))
|
||||
{
|
||||
restitution = 0.f;
|
||||
};
|
||||
}
|
||||
|
||||
/*
|
||||
///warm starting (or zero if disabled)
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
||||
|
||||
if (constraintRow.m_appliedImpulse)
|
||||
{
|
||||
if (multiBodyA)
|
||||
{
|
||||
btScalar impulse = constraintRow.m_appliedImpulse;
|
||||
btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
||||
multiBodyA->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA);
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
btScalar impulse = constraintRow.m_appliedImpulse;
|
||||
btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
|
||||
multiBodyB->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
*/
|
||||
{
|
||||
constraintRow.m_appliedImpulse = 0.f;
|
||||
}
|
||||
|
||||
constraintRow.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = restitution - rel_vel;// * damping;
|
||||
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
|
||||
// const btScalar ALLOWED_PENETRATION = btScalar(0.01);
|
||||
|
||||
// float baumgarte_coeff = 0.3;
|
||||
/// float one_over_dt = 1.f/infoGlobal.m_timeStep;
|
||||
// btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt;
|
||||
// float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
|
||||
|
||||
// solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
|
||||
//solverConstraint.m_rhsPenetration = 0.f;
|
||||
|
||||
//penetration=0.f;
|
||||
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
velocityError = -penetration / infoGlobal.m_timeStep;
|
||||
|
||||
} else
|
||||
{
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
}
|
||||
|
||||
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
|
||||
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
constraintRow.m_rhs = velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = penetrationImpulse;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
constraintRow.m_cfm = 0.f;
|
||||
constraintRow.m_lowerLimit = 0;
|
||||
constraintRow.m_upperLimit = 1e10f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
@ -1013,19 +764,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
|
||||
for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
|
||||
{
|
||||
btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
|
||||
c->update();
|
||||
|
||||
for (int row=0;row<c->getNumRows();row++)
|
||||
{
|
||||
|
||||
|
||||
btMultiBodySolverConstraint& constraintRow = m_multiBodyNonContactConstraints.expandNonInitializing();
|
||||
constraintRow.m_multiBodyA = c->getMultiBodyA();
|
||||
constraintRow.m_multiBodyB = c->getMultiBodyB();
|
||||
|
||||
btScalar penetration = c->getPosition(row);//rhs = c->computeRhs(row,infoGlobal.m_timeStep);
|
||||
setupMultiBodyJointLimitConstraint(constraintRow,c->jacobianA(row),c->jacobianB(row),penetration,0,0,infoGlobal);
|
||||
}
|
||||
c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -21,7 +21,10 @@ subject to the following restrictions:
|
||||
|
||||
|
||||
class btMultiBody;
|
||||
class btMultiBodyConstraint;
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
@ -33,15 +36,7 @@ protected:
|
||||
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
|
||||
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
|
||||
|
||||
|
||||
btAlignedObjectArray<btScalar> m_jacobians;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocities;
|
||||
|
||||
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
btAlignedObjectArray<btMatrix3x3> scratch_m;
|
||||
btMultiBodyJacobianData m_data;
|
||||
|
||||
//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
|
||||
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
|
||||
|
||||
@ -4,15 +4,16 @@
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
|
||||
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
|
||||
:btMultiBodyConstraint(body,body,link,link,2,true),
|
||||
m_lowerBound(lower),
|
||||
m_upperBound(upper)
|
||||
{
|
||||
// the jacobians never change, so may as well
|
||||
// the data.m_jacobians never change, so may as well
|
||||
// initialize them here
|
||||
|
||||
// note: we rely on the fact that jacobians are
|
||||
// note: we rely on the fact that data.m_jacobians are
|
||||
// always initialized to zero by the Constraint ctor
|
||||
|
||||
// row 0: the lower bound
|
||||
@ -35,9 +36,12 @@ int btMultiBodyJointLimitConstraint::getIslandIdB() const
|
||||
return m_bodyB->getLinkCollider(0)->getIslandTag();
|
||||
}
|
||||
|
||||
void btMultiBodyJointLimitConstraint::update()
|
||||
|
||||
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// only positions need to be updated -- jacobians and force
|
||||
// only positions need to be updated -- data.m_jacobians and force
|
||||
// directions were set in the ctor and never change.
|
||||
|
||||
// row 0: the lower bound
|
||||
@ -45,4 +49,249 @@ void btMultiBodyJointLimitConstraint::update()
|
||||
|
||||
// row 1: the upper bound
|
||||
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
|
||||
}
|
||||
|
||||
for (int row=0;row<getNumRows();row++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
constraintRow.m_multiBodyA = m_bodyA;
|
||||
constraintRow.m_multiBodyB = m_bodyB;
|
||||
btScalar penetration = getPosition(row);
|
||||
fillConstraintRow(constraintRow,data,jacobianA(row),jacobianB(row),penetration,0,0,infoGlobal);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA,btScalar* jacOrgB,
|
||||
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
|
||||
{
|
||||
|
||||
|
||||
|
||||
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
|
||||
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
|
||||
const int ndofA = multiBodyA->getNumLinks() + 6;
|
||||
|
||||
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
|
||||
if (constraintRow.m_deltaVelAindex <0)
|
||||
{
|
||||
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
|
||||
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
|
||||
} else
|
||||
{
|
||||
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
|
||||
}
|
||||
|
||||
constraintRow.m_jacAindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
||||
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
||||
for (int i=0;i<ndofA;i++)
|
||||
data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
|
||||
|
||||
float* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
||||
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
}
|
||||
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
|
||||
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
if (constraintRow.m_deltaVelBindex <0)
|
||||
{
|
||||
constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
|
||||
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
|
||||
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
constraintRow.m_jacBindex = data.m_jacobians.size();
|
||||
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
|
||||
|
||||
for (int i=0;i<ndofB;i++)
|
||||
data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
|
||||
|
||||
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
||||
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
|
||||
}
|
||||
{
|
||||
|
||||
btVector3 vec;
|
||||
btScalar denom0 = 0.f;
|
||||
btScalar denom1 = 0.f;
|
||||
btScalar* jacB = 0;
|
||||
btScalar* jacA = 0;
|
||||
btScalar* lambdaA =0;
|
||||
btScalar* lambdaB =0;
|
||||
int ndofA = 0;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumLinks() + 6;
|
||||
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
|
||||
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
float j = jacA[i] ;
|
||||
float l =lambdaA[i];
|
||||
denom0 += j*l;
|
||||
}
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
|
||||
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
float j = jacB[i] ;
|
||||
float l =lambdaB[i];
|
||||
denom1 += j*l;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (multiBodyA && (multiBodyA==multiBodyB))
|
||||
{
|
||||
// ndof1 == ndof2 in this case
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
denom1 += jacB[i] * lambdaA[i];
|
||||
denom1 += jacA[i] * lambdaB[i];
|
||||
}
|
||||
}
|
||||
|
||||
float d = denom0+denom1;
|
||||
if (btFabs(d)>SIMD_EPSILON)
|
||||
{
|
||||
|
||||
constraintRow.m_jacDiagABInv = 1.f/(d);
|
||||
} else
|
||||
{
|
||||
constraintRow.m_jacDiagABInv = 1.f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
//compute rhs and remaining constraintRow fields
|
||||
|
||||
|
||||
|
||||
|
||||
btScalar rel_vel = 0.f;
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
{
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumLinks() + 6;
|
||||
btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
|
||||
for (int i = 0; i < ndofA ; ++i)
|
||||
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
ndofB = multiBodyB->getNumLinks() + 6;
|
||||
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
|
||||
for (int i = 0; i < ndofB ; ++i)
|
||||
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
||||
|
||||
}
|
||||
|
||||
constraintRow.m_friction = combinedFrictionCoeff;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
///warm starting (or zero if disabled)
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
||||
|
||||
if (constraintRow.m_appliedImpulse)
|
||||
{
|
||||
if (multiBodyA)
|
||||
{
|
||||
btScalar impulse = constraintRow.m_appliedImpulse;
|
||||
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
||||
multiBodyA->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA);
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
btScalar impulse = constraintRow.m_appliedImpulse;
|
||||
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
|
||||
multiBodyB->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
*/
|
||||
{
|
||||
constraintRow.m_appliedImpulse = 0.f;
|
||||
}
|
||||
|
||||
constraintRow.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
float desiredVelocity = -0.3;
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = - rel_vel;// * damping;
|
||||
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
velocityError = -penetration / infoGlobal.m_timeStep;
|
||||
} else
|
||||
{
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
}
|
||||
|
||||
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
|
||||
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
constraintRow.m_rhs = velocityImpulse;
|
||||
constraintRow.m_rhsPenetration = penetrationImpulse;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
constraintRow.m_cfm = 0.f;
|
||||
constraintRow.m_lowerLimit = 0;
|
||||
constraintRow.m_upperLimit = 1e10f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -17,6 +17,7 @@ subject to the following restrictions:
|
||||
#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
struct btSolverInfo;
|
||||
|
||||
class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
|
||||
{
|
||||
@ -24,6 +25,11 @@ protected:
|
||||
|
||||
btScalar m_lowerBound;
|
||||
btScalar m_upperBound;
|
||||
void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow,
|
||||
btMultiBodyJacobianData& data,
|
||||
btScalar* jacOrgA,btScalar* jacOrgB,
|
||||
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
|
||||
const btContactSolverInfo& infoGlobal1);
|
||||
public:
|
||||
|
||||
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
|
||||
@ -32,8 +38,11 @@ public:
|
||||
virtual int getIslandIdA() const;
|
||||
virtual int getIslandIdB() const;
|
||||
|
||||
virtual void update();
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
|
||||
|
||||
@ -20,6 +20,8 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
class btMultiBody;
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
|
||||
|
||||
Loading…
Reference in New Issue
Block a user