Merge pull request #410 from erwincoumans/master
support joint force/torque feedback for btMultiBody internal joints (mobilizers)
This commit is contained in:
erwincoumans
181
examples/Constraints/TestHingeTorque.cpp
Normal file
181
examples/Constraints/TestHingeTorque.cpp
Normal file
@@ -0,0 +1,181 @@
|
|||||||
|
#include "TestHingeTorque.h"
|
||||||
|
|
||||||
|
|
||||||
|
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||||
|
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||||
|
|
||||||
|
short collisionFilterGroup = short(btBroadphaseProxy::CharacterFilter);
|
||||||
|
short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::StaticFilter|btBroadphaseProxy::CharacterFilter));
|
||||||
|
|
||||||
|
|
||||||
|
struct TestHingeTorque : public CommonRigidBodyBase
|
||||||
|
{
|
||||||
|
bool m_once;
|
||||||
|
btAlignedObjectArray<btJointFeedback*> m_jointFeedback;
|
||||||
|
|
||||||
|
TestHingeTorque(struct GUIHelperInterface* helper);
|
||||||
|
virtual ~ TestHingeTorque();
|
||||||
|
virtual void initPhysics();
|
||||||
|
|
||||||
|
virtual void stepSimulation(float deltaTime);
|
||||||
|
|
||||||
|
|
||||||
|
virtual void resetCamera()
|
||||||
|
{
|
||||||
|
|
||||||
|
float dist = 5;
|
||||||
|
float pitch = 270;
|
||||||
|
float yaw = 21;
|
||||||
|
float targetPos[3]={-1.34,3.4,-0.44};
|
||||||
|
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
TestHingeTorque::TestHingeTorque(struct GUIHelperInterface* helper)
|
||||||
|
:CommonRigidBodyBase(helper),
|
||||||
|
m_once(true)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
TestHingeTorque::~ TestHingeTorque()
|
||||||
|
{
|
||||||
|
for (int i=0;i<m_jointFeedback.size();i++)
|
||||||
|
{
|
||||||
|
delete m_jointFeedback[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void TestHingeTorque::stepSimulation(float deltaTime)
|
||||||
|
{
|
||||||
|
if (m_once)
|
||||||
|
{
|
||||||
|
m_once=false;
|
||||||
|
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
|
||||||
|
|
||||||
|
btRigidBody& bodyA = hinge->getRigidBodyA();
|
||||||
|
btTransform trA = bodyA.getWorldTransform();
|
||||||
|
btVector3 hingeAxisInWorld = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2);
|
||||||
|
hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*10);
|
||||||
|
hinge->getRigidBodyB().applyTorque(hingeAxisInWorld*10);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
m_dynamicsWorld->stepSimulation(1./60,0);
|
||||||
|
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
|
||||||
|
|
||||||
|
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
|
||||||
|
base->getAngularVelocity()[1],
|
||||||
|
|
||||||
|
base->getAngularVelocity()[2]);
|
||||||
|
|
||||||
|
btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
|
||||||
|
|
||||||
|
b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
|
||||||
|
child->getAngularVelocity()[1],
|
||||||
|
|
||||||
|
child->getAngularVelocity()[2]);
|
||||||
|
|
||||||
|
for (int i=0;i<m_jointFeedback.size();i++)
|
||||||
|
{
|
||||||
|
b3Printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
|
||||||
|
m_jointFeedback[i]->m_appliedForceBodyA.x(),
|
||||||
|
m_jointFeedback[i]->m_appliedForceBodyA.y(),
|
||||||
|
m_jointFeedback[i]->m_appliedForceBodyA.z(),
|
||||||
|
m_jointFeedback[i]->m_appliedTorqueBodyA.x(),
|
||||||
|
m_jointFeedback[i]->m_appliedTorqueBodyA.y(),
|
||||||
|
m_jointFeedback[i]->m_appliedTorqueBodyA.z(),
|
||||||
|
m_jointFeedback[i]->m_appliedForceBodyB.x(),
|
||||||
|
m_jointFeedback[i]->m_appliedForceBodyB.y(),
|
||||||
|
m_jointFeedback[i]->m_appliedForceBodyB.z(),
|
||||||
|
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
|
||||||
|
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
|
||||||
|
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
|
||||||
|
}
|
||||||
|
//CommonRigidBodyBase::stepSimulation(deltaTime);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void TestHingeTorque::initPhysics()
|
||||||
|
{
|
||||||
|
m_guiHelper->setUpAxis(1);
|
||||||
|
|
||||||
|
createEmptyDynamicsWorld();
|
||||||
|
// m_dynamicsWorld->setGravity(btVector3(0,0,0));
|
||||||
|
|
||||||
|
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||||
|
int mode = btIDebugDraw::DBG_DrawWireframe
|
||||||
|
+btIDebugDraw::DBG_DrawConstraints
|
||||||
|
+btIDebugDraw::DBG_DrawConstraintLimits;
|
||||||
|
m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode);
|
||||||
|
|
||||||
|
|
||||||
|
{ // create a door using hinge constraint attached to the world
|
||||||
|
|
||||||
|
int numLinks = 1;
|
||||||
|
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
|
||||||
|
bool canSleep = false;
|
||||||
|
bool selfCollide = false;
|
||||||
|
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
|
||||||
|
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
|
||||||
|
|
||||||
|
btBoxShape* baseBox = new btBoxShape(baseHalfExtents);
|
||||||
|
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
|
||||||
|
btTransform baseWorldTrans;
|
||||||
|
baseWorldTrans.setIdentity();
|
||||||
|
baseWorldTrans.setOrigin(basePosition);
|
||||||
|
|
||||||
|
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
|
||||||
|
//init the base
|
||||||
|
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
|
||||||
|
float baseMass = 0.f;
|
||||||
|
float linkMass = 1.f;
|
||||||
|
|
||||||
|
btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox);
|
||||||
|
m_dynamicsWorld->removeRigidBody(base);
|
||||||
|
base->setDamping(0,0);
|
||||||
|
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
|
||||||
|
btBoxShape* linkBox = new btBoxShape(linkHalfExtents);
|
||||||
|
btRigidBody* prevBody = base;
|
||||||
|
|
||||||
|
for (int i=0;i<numLinks;i++)
|
||||||
|
{
|
||||||
|
btTransform linkTrans;
|
||||||
|
linkTrans = baseWorldTrans;
|
||||||
|
|
||||||
|
linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0));
|
||||||
|
|
||||||
|
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,linkBox);
|
||||||
|
m_dynamicsWorld->removeRigidBody(linkBody);
|
||||||
|
m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
|
||||||
|
linkBody->setDamping(0,0);
|
||||||
|
//create a hinge constraint
|
||||||
|
btVector3 pivotInA(0,-linkHalfExtents[1],0);
|
||||||
|
btVector3 pivotInB(0,linkHalfExtents[1],0);
|
||||||
|
btVector3 axisInA(1,0,0);
|
||||||
|
btVector3 axisInB(1,0,0);
|
||||||
|
bool useReferenceA = true;
|
||||||
|
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
|
||||||
|
pivotInA,pivotInB,
|
||||||
|
axisInA,axisInB,useReferenceA);
|
||||||
|
btJointFeedback* fb = new btJointFeedback();
|
||||||
|
m_jointFeedback.push_back(fb);
|
||||||
|
hinge->setJointFeedback(fb);
|
||||||
|
|
||||||
|
m_dynamicsWorld->addConstraint(hinge,true);
|
||||||
|
prevBody = linkBody;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
|
||||||
|
}
|
||||||
|
|
||||||
|
class CommonExampleInterface* TestHingeTorqueCreateFunc(CommonExampleOptions& options)
|
||||||
|
{
|
||||||
|
return new TestHingeTorque(options.m_guiHelper);
|
||||||
|
}
|
||||||
7
examples/Constraints/TestHingeTorque.h
Normal file
7
examples/Constraints/TestHingeTorque.h
Normal file
@@ -0,0 +1,7 @@
|
|||||||
|
#ifndef TEST_HINGE_TORQUE_H
|
||||||
|
#define TEST_HINGE_TORQUE_H
|
||||||
|
|
||||||
|
class CommonExampleInterface* TestHingeTorqueCreateFunc(struct CommonExampleOptions& options);
|
||||||
|
|
||||||
|
#endif //TEST_HINGE_TORQUE_H
|
||||||
|
|
||||||
@@ -97,6 +97,8 @@ SET(App_ExampleBrowser_SRCS
|
|||||||
../MultiBody/TestJointTorqueSetup.h
|
../MultiBody/TestJointTorqueSetup.h
|
||||||
../MultiBody/MultiDofDemo.cpp
|
../MultiBody/MultiDofDemo.cpp
|
||||||
../MultiBody/MultiDofDemo.h
|
../MultiBody/MultiDofDemo.h
|
||||||
|
../Constraints/TestHingeTorque.cpp
|
||||||
|
../Constraints/TestHingeTorque.h
|
||||||
../Constraints/ConstraintDemo.cpp
|
../Constraints/ConstraintDemo.cpp
|
||||||
../Constraints/ConstraintDemo.h
|
../Constraints/ConstraintDemo.h
|
||||||
../Constraints/Dof6Spring2Setup.cpp
|
../Constraints/Dof6Spring2Setup.cpp
|
||||||
|
|||||||
@@ -32,6 +32,8 @@
|
|||||||
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
|
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
|
||||||
#include "../SharedMemory/PhysicsServer.h"
|
#include "../SharedMemory/PhysicsServer.h"
|
||||||
#include "../SharedMemory/PhysicsClient.h"
|
#include "../SharedMemory/PhysicsClient.h"
|
||||||
|
#include "../Constraints/TestHingeTorque.h"
|
||||||
|
|
||||||
|
|
||||||
#ifdef ENABLE_LUA
|
#ifdef ENABLE_LUA
|
||||||
#include "../LuaDemo/LuaPhysicsSetup.h"
|
#include "../LuaDemo/LuaPhysicsSetup.h"
|
||||||
@@ -79,6 +81,8 @@ static ExampleEntry gDefaultExamples[]=
|
|||||||
AllConstraintCreateFunc),
|
AllConstraintCreateFunc),
|
||||||
|
|
||||||
ExampleEntry(1,"Motorized Hinge","Use of a btHingeConstraint. You can adjust the first slider to change the target velocity, and the second slider to adjust the maximum impulse applied to reach the target velocity. Note that the hinge angle can reach beyond -360 and 360 degrees.", ConstraintCreateFunc),
|
ExampleEntry(1,"Motorized Hinge","Use of a btHingeConstraint. You can adjust the first slider to change the target velocity, and the second slider to adjust the maximum impulse applied to reach the target velocity. Note that the hinge angle can reach beyond -360 and 360 degrees.", ConstraintCreateFunc),
|
||||||
|
ExampleEntry(1,"TestHingeTorque", "Apply a torque in the hinge axis. This example uses a btHingeConstraint and btRigidBody. The setup is similar to the multi body example TestJointTorque.",
|
||||||
|
TestHingeTorqueCreateFunc),
|
||||||
|
|
||||||
// ExampleEntry(0,"What's new in 2.83"),
|
// ExampleEntry(0,"What's new in 2.83"),
|
||||||
|
|
||||||
@@ -94,7 +98,7 @@ static ExampleEntry gDefaultExamples[]=
|
|||||||
|
|
||||||
ExampleEntry(0,"MultiBody"),
|
ExampleEntry(0,"MultiBody"),
|
||||||
ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
|
ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
|
||||||
ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers).", TestJointTorqueCreateFunc),
|
ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
|
||||||
|
|
||||||
|
|
||||||
#ifdef INCLUDE_CLOTH_DEMOS
|
#ifdef INCLUDE_CLOTH_DEMOS
|
||||||
|
|||||||
@@ -2,13 +2,18 @@
|
|||||||
#include "TestJointTorqueSetup.h"
|
#include "TestJointTorqueSetup.h"
|
||||||
|
|
||||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||||
|
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
|
||||||
|
|
||||||
#include "../CommonInterfaces/CommonMultiBodyBase.h"
|
#include "../CommonInterfaces/CommonMultiBodyBase.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
struct TestJointTorqueSetup : public CommonMultiBodyBase
|
struct TestJointTorqueSetup : public CommonMultiBodyBase
|
||||||
{
|
{
|
||||||
btMultiBody* m_multiBody;
|
btMultiBody* m_multiBody;
|
||||||
|
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
|
||||||
|
|
||||||
|
bool m_once;
|
||||||
public:
|
public:
|
||||||
|
|
||||||
TestJointTorqueSetup(struct GUIHelperInterface* helper);
|
TestJointTorqueSetup(struct GUIHelperInterface* helper);
|
||||||
@@ -31,7 +36,8 @@ public:
|
|||||||
};
|
};
|
||||||
|
|
||||||
TestJointTorqueSetup::TestJointTorqueSetup(struct GUIHelperInterface* helper)
|
TestJointTorqueSetup::TestJointTorqueSetup(struct GUIHelperInterface* helper)
|
||||||
:CommonMultiBodyBase(helper)
|
:CommonMultiBodyBase(helper),
|
||||||
|
m_once(true)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -42,7 +48,7 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
|
|||||||
|
|
||||||
void TestJointTorqueSetup::initPhysics()
|
void TestJointTorqueSetup::initPhysics()
|
||||||
{
|
{
|
||||||
int upAxis = 2;
|
int upAxis = 1;
|
||||||
m_guiHelper->setUpAxis(upAxis);
|
m_guiHelper->setUpAxis(upAxis);
|
||||||
|
|
||||||
btVector4 colors[4] =
|
btVector4 colors[4] =
|
||||||
@@ -67,8 +73,10 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
+btIDebugDraw::DBG_DrawAabb
|
+btIDebugDraw::DBG_DrawAabb
|
||||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//create a static ground object
|
//create a static ground object
|
||||||
if (0)
|
if (1)
|
||||||
{
|
{
|
||||||
btVector3 groundHalfExtents(20,20,20);
|
btVector3 groundHalfExtents(20,20,20);
|
||||||
groundHalfExtents[upAxis]=1.f;
|
groundHalfExtents[upAxis]=1.f;
|
||||||
@@ -90,8 +98,8 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
{
|
{
|
||||||
bool floating = false;
|
bool floating = false;
|
||||||
bool damping = true;
|
bool damping = true;
|
||||||
bool gyro = true;
|
bool gyro = false;
|
||||||
int numLinks = 5;
|
int numLinks = 1;
|
||||||
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
|
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
|
||||||
bool canSleep = false;
|
bool canSleep = false;
|
||||||
bool selfCollide = false;
|
bool selfCollide = false;
|
||||||
@@ -106,15 +114,18 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
|
|
||||||
if(baseMass)
|
if(baseMass)
|
||||||
{
|
{
|
||||||
btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
|
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
|
||||||
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
|
btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
|
||||||
delete pTempBox;
|
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
|
||||||
|
delete shape;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isMultiDof = false;
|
bool isMultiDof = true;
|
||||||
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
|
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
|
||||||
|
|
||||||
m_multiBody = pMultiBody;
|
m_multiBody = pMultiBody;
|
||||||
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
|
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
|
||||||
|
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
|
||||||
pMultiBody->setBasePos(basePosition);
|
pMultiBody->setBasePos(basePosition);
|
||||||
pMultiBody->setWorldToBaseRot(baseOriQuat);
|
pMultiBody->setWorldToBaseRot(baseOriQuat);
|
||||||
btVector3 vel(0, 0, 0);
|
btVector3 vel(0, 0, 0);
|
||||||
@@ -122,12 +133,6 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
|
|
||||||
//init the links
|
//init the links
|
||||||
btVector3 hingeJointAxis(1, 0, 0);
|
btVector3 hingeJointAxis(1, 0, 0);
|
||||||
float linkMass = 1.f;
|
|
||||||
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
|
|
||||||
|
|
||||||
btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
|
|
||||||
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
|
|
||||||
delete pTempBox;
|
|
||||||
|
|
||||||
//y-axis assumed up
|
//y-axis assumed up
|
||||||
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
|
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
|
||||||
@@ -142,15 +147,46 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
|
|
||||||
for(int i = 0; i < numLinks; ++i)
|
for(int i = 0; i < numLinks; ++i)
|
||||||
{
|
{
|
||||||
|
float linkMass = 1.f;
|
||||||
|
//if (i==3 || i==2)
|
||||||
|
// linkMass= 1000;
|
||||||
|
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
|
||||||
|
|
||||||
|
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//new btSphereShape(linkHalfExtents[0]);
|
||||||
|
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
|
||||||
|
delete shape;
|
||||||
|
|
||||||
|
|
||||||
if(!spherical)
|
if(!spherical)
|
||||||
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
{
|
||||||
|
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
||||||
|
|
||||||
|
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
|
||||||
|
btQuaternion(0.f, 0.f, 0.f, 1.f),
|
||||||
|
hingeJointAxis,
|
||||||
|
parentComToCurrentPivot,
|
||||||
|
currentPivotToCurrentCom, false);
|
||||||
|
|
||||||
|
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
|
||||||
|
|
||||||
|
}
|
||||||
else
|
else
|
||||||
|
{
|
||||||
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
|
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
|
||||||
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
//pMultiBody->finalizeMultiDof();
|
pMultiBody->finalizeMultiDof();
|
||||||
|
|
||||||
|
//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
|
||||||
|
for (int i=0;i<pMultiBody->getNumLinks();i++)
|
||||||
|
{
|
||||||
|
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
|
||||||
|
pMultiBody->getLink(i).m_jointFeedback = fb;
|
||||||
|
m_jointFeedbacks.push_back(fb);
|
||||||
|
//break;
|
||||||
|
}
|
||||||
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
|
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
|
||||||
|
|
||||||
///
|
///
|
||||||
@@ -170,10 +206,11 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
}
|
}
|
||||||
//
|
//
|
||||||
btVector3 gravity(0,0,0);
|
btVector3 gravity(0,0,0);
|
||||||
//gravity[upAxis] = -9.81;
|
gravity[upAxis] = -9.81;
|
||||||
|
gravity[0] = 0;
|
||||||
m_dynamicsWorld->setGravity(gravity);
|
m_dynamicsWorld->setGravity(gravity);
|
||||||
//////////////////////////////////////////////
|
//////////////////////////////////////////////
|
||||||
if(numLinks > 0)
|
if(0)//numLinks > 0)
|
||||||
{
|
{
|
||||||
btScalar q0 = 45.f * SIMD_PI/ 180.f;
|
btScalar q0 = 45.f * SIMD_PI/ 180.f;
|
||||||
if(!spherical)
|
if(!spherical)
|
||||||
@@ -206,11 +243,11 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
|
|
||||||
if (1)
|
if (1)
|
||||||
{
|
{
|
||||||
btCollisionShape* box = new btBoxShape(baseHalfExtents);
|
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
|
||||||
m_guiHelper->createCollisionShapeGraphicsObject(box);
|
m_guiHelper->createCollisionShapeGraphicsObject(shape);
|
||||||
|
|
||||||
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
|
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
|
||||||
col->setCollisionShape(box);
|
col->setCollisionShape(shape);
|
||||||
|
|
||||||
btTransform tr;
|
btTransform tr;
|
||||||
tr.setIdentity();
|
tr.setIdentity();
|
||||||
@@ -221,7 +258,12 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||||
col->setWorldTransform(tr);
|
col->setWorldTransform(tr);
|
||||||
|
|
||||||
world->addCollisionObject(col, 2,1+2);
|
bool isDynamic = (baseMass > 0 && floating);
|
||||||
|
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
|
||||||
|
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||||
|
|
||||||
|
|
||||||
|
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
|
||||||
|
|
||||||
btVector3 color(0.0,0.0,0.5);
|
btVector3 color(0.0,0.0,0.5);
|
||||||
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
|
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
|
||||||
@@ -249,18 +291,24 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
|
|
||||||
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()};
|
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(linkHalfExtents);
|
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
|
||||||
m_guiHelper->createCollisionShapeGraphicsObject(box);
|
m_guiHelper->createCollisionShapeGraphicsObject(shape);
|
||||||
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
|
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
|
||||||
|
|
||||||
col->setCollisionShape(box);
|
col->setCollisionShape(shape);
|
||||||
btTransform tr;
|
btTransform tr;
|
||||||
tr.setIdentity();
|
tr.setIdentity();
|
||||||
tr.setOrigin(posr);
|
tr.setOrigin(posr);
|
||||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||||
col->setWorldTransform(tr);
|
col->setWorldTransform(tr);
|
||||||
col->setFriction(friction);
|
col->setFriction(friction);
|
||||||
world->addCollisionObject(col,2,1+2);
|
bool isDynamic = 1;//(linkMass > 0);
|
||||||
|
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
|
||||||
|
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||||
|
|
||||||
|
//if (i==0||i>numLinks-2)
|
||||||
|
{
|
||||||
|
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
|
||||||
btVector4 color = colors[curColor];
|
btVector4 color = colors[curColor];
|
||||||
curColor++;
|
curColor++;
|
||||||
curColor&=3;
|
curColor&=3;
|
||||||
@@ -269,14 +317,56 @@ void TestJointTorqueSetup::initPhysics()
|
|||||||
|
|
||||||
pMultiBody->getLink(i).m_collider=col;
|
pMultiBody->getLink(i).m_collider=col;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void TestJointTorqueSetup::stepSimulation(float deltaTime)
|
void TestJointTorqueSetup::stepSimulation(float deltaTime)
|
||||||
{
|
{
|
||||||
|
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
|
||||||
|
if (0)//m_once)
|
||||||
|
{
|
||||||
|
m_once=false;
|
||||||
m_multiBody->addJointTorque(0, 10.0);
|
m_multiBody->addJointTorque(0, 10.0);
|
||||||
m_dynamicsWorld->stepSimulation(deltaTime);
|
|
||||||
|
btScalar torque = m_multiBody->getJointTorque(0);
|
||||||
|
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_dynamicsWorld->stepSimulation(1./60,0);
|
||||||
|
|
||||||
|
if (1)
|
||||||
|
for (int i=0;i<m_jointFeedbacks.size();i++)
|
||||||
|
{
|
||||||
|
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
|
||||||
|
i,
|
||||||
|
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
|
||||||
|
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
|
||||||
|
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
|
||||||
|
|
||||||
|
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
|
||||||
|
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
|
||||||
|
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
|
||||||
|
|
||||||
|
);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
|
||||||
|
m_multiBody->getBaseOmega()[1],
|
||||||
|
m_multiBody->getBaseOmega()[2]
|
||||||
|
);
|
||||||
|
*/
|
||||||
|
btScalar jointVel =m_multiBody->getJointVel(0);
|
||||||
|
|
||||||
|
// b3Printf("child angvel = %f",jointVel);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -24,8 +24,10 @@
|
|||||||
#include "btMultiBody.h"
|
#include "btMultiBody.h"
|
||||||
#include "btMultiBodyLink.h"
|
#include "btMultiBodyLink.h"
|
||||||
#include "btMultiBodyLinkCollider.h"
|
#include "btMultiBodyLinkCollider.h"
|
||||||
|
#include "btMultiBodyJointFeedback.h"
|
||||||
#include "LinearMath/btTransformUtil.h"
|
#include "LinearMath/btTransformUtil.h"
|
||||||
|
|
||||||
|
#include "Bullet3Common/b3Logging.h"
|
||||||
// #define INCLUDE_GYRO_TERM
|
// #define INCLUDE_GYRO_TERM
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
@@ -110,7 +112,8 @@ btMultiBody::btMultiBody(int n_links,
|
|||||||
m_dofCount(0),
|
m_dofCount(0),
|
||||||
m_posVarCnt(0),
|
m_posVarCnt(0),
|
||||||
m_useRK4(false),
|
m_useRK4(false),
|
||||||
m_useGlobalVelocities(false)
|
m_useGlobalVelocities(false),
|
||||||
|
m_internalNeedsJointFeedback(false)
|
||||||
{
|
{
|
||||||
|
|
||||||
if(!m_isMultiDof)
|
if(!m_isMultiDof)
|
||||||
@@ -298,7 +301,6 @@ void btMultiBody::setupSpherical(int i,
|
|||||||
updateLinksDofOffsets();
|
updateLinksDofOffsets();
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
|
||||||
void btMultiBody::setupPlanar(int i,
|
void btMultiBody::setupPlanar(int i,
|
||||||
btScalar mass,
|
btScalar mass,
|
||||||
const btVector3 &inertia,
|
const btVector3 &inertia,
|
||||||
@@ -347,7 +349,6 @@ void btMultiBody::setupPlanar(int i,
|
|||||||
//
|
//
|
||||||
updateLinksDofOffsets();
|
updateLinksDofOffsets();
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
void btMultiBody::finalizeMultiDof()
|
void btMultiBody::finalizeMultiDof()
|
||||||
{
|
{
|
||||||
@@ -629,7 +630,6 @@ inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //r
|
|||||||
#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
|
#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
|
||||||
//
|
//
|
||||||
|
|
||||||
#ifndef TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||||
btAlignedObjectArray<btScalar> &scratch_r,
|
btAlignedObjectArray<btScalar> &scratch_r,
|
||||||
btAlignedObjectArray<btVector3> &scratch_v,
|
btAlignedObjectArray<btVector3> &scratch_v,
|
||||||
@@ -645,450 +645,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
|||||||
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
|
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
|
||||||
// num_links joint acceleration values.
|
// num_links joint acceleration values.
|
||||||
|
|
||||||
int num_links = getNumLinks();
|
m_internalNeedsJointFeedback = false;
|
||||||
|
|
||||||
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
|
|
||||||
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
|
|
||||||
|
|
||||||
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
|
|
||||||
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
|
|
||||||
|
|
||||||
btVector3 base_vel = getBaseVel();
|
|
||||||
btVector3 base_omega = getBaseOmega();
|
|
||||||
|
|
||||||
// Temporary matrices/vectors -- use scratch space from caller
|
|
||||||
// so that we don't have to keep reallocating every frame
|
|
||||||
|
|
||||||
scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
|
|
||||||
scratch_v.resize(8*num_links + 6);
|
|
||||||
scratch_m.resize(4*num_links + 4);
|
|
||||||
|
|
||||||
btScalar * r_ptr = &scratch_r[0];
|
|
||||||
btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
|
|
||||||
btVector3 * v_ptr = &scratch_v[0];
|
|
||||||
|
|
||||||
// vhat_i (top = angular, bottom = linear part)
|
|
||||||
btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
|
|
||||||
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
|
||||||
|
|
||||||
// zhat_i^A
|
|
||||||
btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
|
|
||||||
btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
|
|
||||||
|
|
||||||
// chat_i (note NOT defined for the base)
|
|
||||||
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
|
|
||||||
btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
|
|
||||||
|
|
||||||
// top left, top right and bottom left blocks of Ihat_i^A.
|
|
||||||
// bottom right block = transpose of top left block and is not stored.
|
|
||||||
// Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
|
|
||||||
btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
|
|
||||||
btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
|
|
||||||
btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
|
|
||||||
|
|
||||||
// Cached 3x3 rotation matrices from parent frame to this frame.
|
|
||||||
btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
|
|
||||||
btMatrix3x3 * rot_from_world = &scratch_m[0];
|
|
||||||
|
|
||||||
// hhat_i, ahat_i
|
|
||||||
// hhat is NOT stored for the base (but ahat is)
|
|
||||||
btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0;
|
|
||||||
btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0;
|
|
||||||
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
|
|
||||||
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
|
||||||
|
|
||||||
// Y_i, invD_i
|
|
||||||
btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
|
|
||||||
btScalar * Y = &scratch_r[0];
|
|
||||||
/////////////////
|
|
||||||
|
|
||||||
// ptr to the joint accel part of the output
|
|
||||||
btScalar * joint_accel = output + 6;
|
|
||||||
|
|
||||||
// Start of the algorithm proper.
|
|
||||||
|
|
||||||
// First 'upward' loop.
|
|
||||||
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
|
||||||
|
|
||||||
rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
|
|
||||||
|
|
||||||
vel_top_angular[0] = rot_from_parent[0] * base_omega;
|
|
||||||
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
|
|
||||||
|
|
||||||
if (m_fixedBase)
|
|
||||||
{
|
|
||||||
zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
zeroAccForce[0] = - (rot_from_parent[0] * (m_baseForce
|
|
||||||
- m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
|
|
||||||
|
|
||||||
zeroAccTorque[0] =
|
|
||||||
- (rot_from_parent[0] * m_baseTorque);
|
|
||||||
|
|
||||||
if (m_useGyroTerm)
|
|
||||||
zeroAccTorque[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
|
|
||||||
|
|
||||||
zeroAccTorque[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
|
|
||||||
|
|
||||||
//NOTE: Coriolis terms are missing! (left like so following Stephen's code)
|
|
||||||
}
|
|
||||||
|
|
||||||
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);
|
|
||||||
|
|
||||||
|
|
||||||
inertia_top_right[0].setValue(m_baseMass, 0, 0,
|
|
||||||
0, m_baseMass, 0,
|
|
||||||
0, 0, m_baseMass);
|
|
||||||
inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
|
|
||||||
0, m_baseInertia[1], 0,
|
|
||||||
0, 0, m_baseInertia[2]);
|
|
||||||
|
|
||||||
rot_from_world[0] = rot_from_parent[0];
|
|
||||||
|
|
||||||
for (int i = 0; i < num_links; ++i) {
|
|
||||||
const int parent = m_links[i].m_parent;
|
|
||||||
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
|
|
||||||
|
|
||||||
|
|
||||||
rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
|
|
||||||
|
|
||||||
// vhat_i = i_xhat_p(i) * vhat_p(i)
|
|
||||||
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
||||||
vel_top_angular[parent+1], vel_bottom_linear[parent+1],
|
|
||||||
vel_top_angular[i+1], vel_bottom_linear[i+1]);
|
|
||||||
//////////////////////////////////////////////////////////////
|
|
||||||
|
|
||||||
// now set vhat_i to its true value by doing
|
|
||||||
// vhat_i += qidot * shat_i
|
|
||||||
btVector3 joint_vel_spat_top, joint_vel_spat_bottom;
|
|
||||||
joint_vel_spat_top.setZero(); joint_vel_spat_bottom.setZero();
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
joint_vel_spat_top += getJointVelMultiDof(i)[dof] * m_links[i].getAxisTop(dof);
|
|
||||||
joint_vel_spat_bottom += getJointVelMultiDof(i)[dof] * m_links[i].getAxisBottom(dof);
|
|
||||||
}
|
|
||||||
|
|
||||||
vel_top_angular[i+1] += joint_vel_spat_top;
|
|
||||||
vel_bottom_linear[i+1] += joint_vel_spat_bottom;
|
|
||||||
|
|
||||||
// we can now calculate chat_i
|
|
||||||
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
|
|
||||||
SpatialCrossProduct(vel_top_angular[i+1], vel_bottom_linear[i+1], joint_vel_spat_top, joint_vel_spat_bottom, coriolis_top_angular[i], coriolis_bottom_linear[i]);
|
|
||||||
|
|
||||||
// calculate zhat_i^A
|
|
||||||
//
|
|
||||||
//external forces
|
|
||||||
zeroAccForce[i+1] = -(rot_from_world[i+1] * (m_links[i].m_appliedForce));
|
|
||||||
zeroAccTorque[i+1] = -(rot_from_world[i+1] * m_links[i].m_appliedTorque);
|
|
||||||
//
|
|
||||||
//DAMPING TERMS (ONLY)
|
|
||||||
zeroAccForce[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
|
|
||||||
zeroAccTorque[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
|
|
||||||
|
|
||||||
// calculate Ihat_i^A
|
|
||||||
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
|
|
||||||
inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
|
|
||||||
0, m_links[i].m_mass, 0,
|
|
||||||
0, 0, m_links[i].m_mass);
|
|
||||||
inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
|
|
||||||
0, m_links[i].m_inertia[1], 0,
|
|
||||||
0, 0, m_links[i].m_inertia[2]);
|
|
||||||
|
|
||||||
|
|
||||||
////
|
|
||||||
//p += v x* Iv - done in a simpler way
|
|
||||||
if(m_useGyroTerm)
|
|
||||||
zeroAccTorque[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
|
|
||||||
//
|
|
||||||
coriolis_bottom_linear[i] += vel_top_angular[i+1].cross(vel_bottom_linear[i+1]) - (rot_from_parent[i+1] * (vel_top_angular[parent+1].cross(vel_bottom_linear[parent+1])));
|
|
||||||
//coriolis_bottom_linear[i].setZero();
|
|
||||||
|
|
||||||
//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
|
|
||||||
//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
|
|
||||||
//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
|
|
||||||
}
|
|
||||||
|
|
||||||
static btScalar D[36]; //it's dofxdof for each body so asingle 6x6 D matrix will do
|
|
||||||
// 'Downward' loop.
|
|
||||||
// (part of TreeForwardDynamics in Mirtich.)
|
|
||||||
for (int i = num_links - 1; i >= 0; --i)
|
|
||||||
{
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
|
||||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
|
||||||
|
|
||||||
//pFunMultSpatVecTimesSpatMat2(m_links[i].m_axesTop[dof], m_links[i].m_axesBottom[dof], inertia_top_left[i+1], inertia_top_right[i+1], inertia_bottom_left[i+1], h_t, h_b);
|
|
||||||
{
|
|
||||||
h_t = inertia_top_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_right[i+1] * m_links[i].getAxisBottom(dof);
|
|
||||||
h_b = inertia_bottom_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(dof);
|
|
||||||
}
|
|
||||||
|
|
||||||
btScalar *D_row = &D[dof * m_links[i].m_dofCount];
|
|
||||||
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
||||||
{
|
|
||||||
D_row[dof2] = SpatialDotProduct(m_links[i].getAxisTop(dof2), m_links[i].getAxisBottom(dof2), h_t, h_b);
|
|
||||||
}
|
|
||||||
|
|
||||||
Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
|
|
||||||
- SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
|
|
||||||
- SpatialDotProduct(h_t, h_b, coriolis_top_angular[i], coriolis_bottom_linear[i])
|
|
||||||
;
|
|
||||||
}
|
|
||||||
|
|
||||||
const int parent = m_links[i].m_parent;
|
|
||||||
|
|
||||||
btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
|
||||||
switch(m_links[i].m_jointType)
|
|
||||||
{
|
|
||||||
case btMultibodyLink::ePrismatic:
|
|
||||||
case btMultibodyLink::eRevolute:
|
|
||||||
{
|
|
||||||
invDi[0] = 1.0f / D[0];
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case btMultibodyLink::eSpherical:
|
|
||||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
|
||||||
case btMultibodyLink::ePlanar:
|
|
||||||
#endif
|
|
||||||
{
|
|
||||||
static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
|
|
||||||
static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
|
|
||||||
|
|
||||||
//unroll the loop?
|
|
||||||
for(int row = 0; row < 3; ++row)
|
|
||||||
for(int col = 0; col < 3; ++col)
|
|
||||||
invDi[row * 3 + col] = invD3x3[row][col];
|
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
default:
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
static btVector3 tmp_top[6]; //move to scratch mem or other buffers? (12 btVector3 will suffice)
|
|
||||||
static btVector3 tmp_bottom[6];
|
|
||||||
|
|
||||||
//for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
//{
|
|
||||||
// tmp_top[dof].setZero();
|
|
||||||
// tmp_bottom[dof].setZero();
|
|
||||||
|
|
||||||
// for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
||||||
// {
|
|
||||||
// btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2];
|
|
||||||
// btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
|
|
||||||
// //
|
|
||||||
// tmp_top[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_b;
|
|
||||||
// tmp_bottom[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_t;
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
|
|
||||||
//btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
|
|
||||||
|
|
||||||
//for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
//{
|
|
||||||
// btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
|
||||||
// btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
|
||||||
|
|
||||||
// TL -= outerProduct(h_t, tmp_top[dof]);
|
|
||||||
// TR -= outerProduct(h_t, tmp_bottom[dof]);
|
|
||||||
// BL -= outerProduct(h_b, tmp_top[dof]);
|
|
||||||
//}
|
|
||||||
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
tmp_top[dof].setZero();
|
|
||||||
tmp_bottom[dof].setZero();
|
|
||||||
|
|
||||||
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
||||||
{
|
|
||||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2];
|
|
||||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
|
|
||||||
//
|
|
||||||
tmp_top[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_t;
|
|
||||||
tmp_bottom[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_b;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
|
|
||||||
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
|
||||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
|
||||||
|
|
||||||
TL -= outerProduct(h_t, tmp_bottom[dof]);
|
|
||||||
TR -= outerProduct(h_t, tmp_top[dof]);
|
|
||||||
BL -= outerProduct(h_b, tmp_bottom[dof]);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
btMatrix3x3 r_cross;
|
|
||||||
r_cross.setValue(
|
|
||||||
0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
|
|
||||||
m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
|
|
||||||
-m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
|
|
||||||
|
|
||||||
inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
|
|
||||||
inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
|
|
||||||
inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
|
|
||||||
(r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
|
|
||||||
|
|
||||||
|
|
||||||
btVector3 in_top, in_bottom, out_top, out_bottom;
|
|
||||||
|
|
||||||
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
invD_times_Y[dof] = 0.f;
|
|
||||||
|
|
||||||
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
||||||
{
|
|
||||||
invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
in_top = zeroAccForce[i+1]
|
|
||||||
+ inertia_top_left[i+1] * coriolis_top_angular[i]
|
|
||||||
+ inertia_top_right[i+1] * coriolis_bottom_linear[i];
|
|
||||||
|
|
||||||
in_bottom = zeroAccTorque[i+1]
|
|
||||||
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
|
|
||||||
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i];
|
|
||||||
|
|
||||||
//unroll the loop?
|
|
||||||
for(int row = 0; row < 3; ++row)
|
|
||||||
{
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
|
||||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
|
||||||
|
|
||||||
in_top[row] += h_t[row] * invD_times_Y[dof];
|
|
||||||
in_bottom[row] += h_b[row] * invD_times_Y[dof];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
||||||
in_top, in_bottom, out_top, out_bottom);
|
|
||||||
|
|
||||||
zeroAccForce[parent+1] += out_top;
|
|
||||||
zeroAccTorque[parent+1] += out_bottom;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Second 'upward' loop
|
|
||||||
// (part of TreeForwardDynamics in Mirtich)
|
|
||||||
|
|
||||||
if (m_fixedBase)
|
|
||||||
{
|
|
||||||
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if (num_links > 0)
|
|
||||||
{
|
|
||||||
m_cachedInertiaTopLeft = inertia_top_left[0];
|
|
||||||
m_cachedInertiaTopRight = inertia_top_right[0];
|
|
||||||
m_cachedInertiaLowerLeft = inertia_bottom_left[0];
|
|
||||||
m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
|
|
||||||
|
|
||||||
}
|
|
||||||
btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
|
|
||||||
btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
|
|
||||||
float result[6];
|
|
||||||
|
|
||||||
solveImatrix(rhs_top, rhs_bot, result);
|
|
||||||
for (int i = 0; i < 3; ++i) {
|
|
||||||
accel_top[0][i] = -result[i];
|
|
||||||
accel_bottom[0][i] = -result[i+3];
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
static btScalar Y_minus_hT_a[6]; //it's dofx1 for each body so a single 6x1 temp is enough
|
|
||||||
// now do the loop over the m_links
|
|
||||||
for (int i = 0; i < num_links; ++i) {
|
|
||||||
const int parent = m_links[i].m_parent;
|
|
||||||
|
|
||||||
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
||||||
accel_top[parent+1], accel_bottom[parent+1],
|
|
||||||
accel_top[i+1], accel_bottom[i+1]);
|
|
||||||
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
|
||||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
|
||||||
|
|
||||||
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
|
||||||
mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
|
|
||||||
|
|
||||||
accel_top[i+1] += coriolis_top_angular[i];
|
|
||||||
accel_bottom[i+1] += coriolis_bottom_linear[i];
|
|
||||||
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof);
|
|
||||||
accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// transform base accelerations back to the world frame.
|
|
||||||
btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
|
|
||||||
output[0] = omegadot_out[0];
|
|
||||||
output[1] = omegadot_out[1];
|
|
||||||
output[2] = omegadot_out[2];
|
|
||||||
|
|
||||||
btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
|
|
||||||
output[3] = vdot_out[0];
|
|
||||||
output[4] = vdot_out[1];
|
|
||||||
output[5] = vdot_out[2];
|
|
||||||
|
|
||||||
/////////////////
|
|
||||||
//printf("q = [");
|
|
||||||
//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
|
|
||||||
//for(int link = 0; link < getNumLinks(); ++link)
|
|
||||||
// for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
|
|
||||||
// printf("%.6f ", m_links[link].m_jointPos[dof]);
|
|
||||||
//printf("]\n");
|
|
||||||
////
|
|
||||||
//printf("qd = [");
|
|
||||||
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
|
||||||
// printf("%.6f ", m_realBuf[dof]);
|
|
||||||
//printf("]\n");
|
|
||||||
//printf("qdd = [");
|
|
||||||
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
|
||||||
// printf("%.6f ", output[dof]);
|
|
||||||
//printf("]\n");
|
|
||||||
/////////////////
|
|
||||||
|
|
||||||
// Final step: add the accelerations (times dt) to the velocities.
|
|
||||||
applyDeltaVeeMultiDof(output, dt);
|
|
||||||
}
|
|
||||||
|
|
||||||
#else //i.e. TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
|
||||||
btAlignedObjectArray<btScalar> &scratch_r,
|
|
||||||
btAlignedObjectArray<btVector3> &scratch_v,
|
|
||||||
btAlignedObjectArray<btMatrix3x3> &scratch_m)
|
|
||||||
{
|
|
||||||
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
|
|
||||||
// and the base linear & angular accelerations.
|
|
||||||
|
|
||||||
// We apply damping forces in this routine as well as any external forces specified by the
|
|
||||||
// caller (via addBaseForce etc).
|
|
||||||
|
|
||||||
// output should point to an array of 6 + num_links reals.
|
|
||||||
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
|
|
||||||
// num_links joint acceleration values.
|
|
||||||
|
|
||||||
int num_links = getNumLinks();
|
int num_links = getNumLinks();
|
||||||
|
|
||||||
@@ -1244,6 +801,20 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
|||||||
//
|
//
|
||||||
//external forces
|
//external forces
|
||||||
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
|
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
|
||||||
|
|
||||||
|
if (0)
|
||||||
|
{
|
||||||
|
|
||||||
|
b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
|
||||||
|
i+1,
|
||||||
|
zeroAccSpatFrc[i+1].m_topVec[0],
|
||||||
|
zeroAccSpatFrc[i+1].m_topVec[1],
|
||||||
|
zeroAccSpatFrc[i+1].m_topVec[2],
|
||||||
|
|
||||||
|
zeroAccSpatFrc[i+1].m_bottomVec[0],
|
||||||
|
zeroAccSpatFrc[i+1].m_bottomVec[1],
|
||||||
|
zeroAccSpatFrc[i+1].m_bottomVec[2]);
|
||||||
|
}
|
||||||
//
|
//
|
||||||
//adding damping terms (only)
|
//adding damping terms (only)
|
||||||
btScalar linDampMult = 1., angDampMult = 1.;
|
btScalar linDampMult = 1., angDampMult = 1.;
|
||||||
@@ -1325,9 +896,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case btMultibodyLink::eSpherical:
|
case btMultibodyLink::eSpherical:
|
||||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
|
||||||
case btMultibodyLink::ePlanar:
|
case btMultibodyLink::ePlanar:
|
||||||
#endif
|
|
||||||
{
|
{
|
||||||
static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
|
static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
|
||||||
static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
|
static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
|
||||||
@@ -1421,6 +990,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
|||||||
spatAcc[0] = -result;
|
spatAcc[0] = -result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// now do the loop over the m_links
|
// now do the loop over the m_links
|
||||||
for (int i = 0; i < num_links; ++i)
|
for (int i = 0; i < num_links; ++i)
|
||||||
{
|
{
|
||||||
@@ -1450,6 +1020,17 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
|||||||
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||||
spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
|
spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
|
||||||
|
|
||||||
|
if (m_links[i].m_jointFeedback)
|
||||||
|
{
|
||||||
|
|
||||||
|
|
||||||
|
m_internalNeedsJointFeedback = true;
|
||||||
|
m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
|
||||||
|
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
|
||||||
|
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// transform base accelerations back to the world frame.
|
// transform base accelerations back to the world frame.
|
||||||
@@ -1546,7 +1127,6 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
|||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
void btMultiBody::stepVelocities(btScalar dt,
|
void btMultiBody::stepVelocities(btScalar dt,
|
||||||
@@ -1591,8 +1171,8 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
||||||
|
|
||||||
// zhat_i^A
|
// zhat_i^A
|
||||||
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
|
btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
|
||||||
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
||||||
|
|
||||||
// chat_i (note NOT defined for the base)
|
// chat_i (note NOT defined for the base)
|
||||||
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
|
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
|
||||||
@@ -1617,7 +1197,7 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
||||||
|
|
||||||
// Y_i, D_i
|
// Y_i, D_i
|
||||||
btScalar * Y = r_ptr; r_ptr += num_links;
|
btScalar * Y1 = r_ptr; r_ptr += num_links;
|
||||||
btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
|
btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
|
||||||
|
|
||||||
// ptr to the joint accel part of the output
|
// ptr to the joint accel part of the output
|
||||||
@@ -1635,18 +1215,18 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
|
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
|
||||||
|
|
||||||
if (m_fixedBase) {
|
if (m_fixedBase) {
|
||||||
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
|
f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0);
|
||||||
} else {
|
} else {
|
||||||
zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
|
f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
|
||||||
- m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
|
- m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
|
||||||
|
|
||||||
zero_acc_bottom_linear[0] =
|
f_zero_acc_bottom_linear[0] =
|
||||||
- (rot_from_parent[0] * m_baseTorque);
|
- (rot_from_parent[0] * m_baseTorque);
|
||||||
|
|
||||||
if (m_useGyroTerm)
|
if (m_useGyroTerm)
|
||||||
zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
|
f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
|
||||||
|
|
||||||
zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
|
f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1693,17 +1273,17 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
|
vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
|
||||||
|
|
||||||
// calculate zhat_i^A
|
// calculate zhat_i^A
|
||||||
zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
|
f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
|
||||||
zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
|
f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
|
||||||
|
|
||||||
zero_acc_bottom_linear[i+1] =
|
f_zero_acc_bottom_linear[i+1] =
|
||||||
- (rot_from_world[i+1] * m_links[i].m_appliedTorque);
|
- (rot_from_world[i+1] * m_links[i].m_appliedTorque);
|
||||||
if (m_useGyroTerm)
|
if (m_useGyroTerm)
|
||||||
{
|
{
|
||||||
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
|
f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
|
||||||
}
|
}
|
||||||
|
|
||||||
zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
|
f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
|
||||||
|
|
||||||
// calculate Ihat_i^A
|
// calculate Ihat_i^A
|
||||||
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
|
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
|
||||||
@@ -1724,9 +1304,9 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
|
h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
|
||||||
btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
|
btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
|
||||||
D[i] = val;
|
D[i] = val;
|
||||||
|
//Y1 = joint torque - zero acceleration force - coriolis force
|
||||||
Y[i] = m_links[i].m_jointTorque[0]
|
Y1[i] = m_links[i].m_jointTorque[0]
|
||||||
- SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
|
- SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1])
|
||||||
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
|
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
|
||||||
|
|
||||||
const int parent = m_links[i].m_parent;
|
const int parent = m_links[i].m_parent;
|
||||||
@@ -1757,19 +1337,19 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
|
|
||||||
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
|
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
|
||||||
btVector3 in_top, in_bottom, out_top, out_bottom;
|
btVector3 in_top, in_bottom, out_top, out_bottom;
|
||||||
const btScalar Y_over_D = Y[i] * one_over_di;
|
const btScalar Y_over_D = Y1[i] * one_over_di;
|
||||||
in_top = zero_acc_top_angular[i+1]
|
in_top = f_zero_acc_top_angular[i+1]
|
||||||
+ inertia_top_left[i+1] * coriolis_top_angular[i]
|
+ inertia_top_left[i+1] * coriolis_top_angular[i]
|
||||||
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
|
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
|
||||||
+ Y_over_D * h_top[i];
|
+ Y_over_D * h_top[i];
|
||||||
in_bottom = zero_acc_bottom_linear[i+1]
|
in_bottom = f_zero_acc_bottom_linear[i+1]
|
||||||
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
|
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
|
||||||
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
|
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
|
||||||
+ Y_over_D * h_bottom[i];
|
+ Y_over_D * h_bottom[i];
|
||||||
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||||
in_top, in_bottom, out_top, out_bottom);
|
in_top, in_bottom, out_top, out_bottom);
|
||||||
zero_acc_top_angular[parent+1] += out_top;
|
f_zero_acc_top_angular[parent+1] += out_top;
|
||||||
zero_acc_bottom_linear[parent+1] += out_bottom;
|
f_zero_acc_bottom_linear[parent+1] += out_bottom;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -1797,8 +1377,8 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
|
m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
|
||||||
|
|
||||||
}
|
}
|
||||||
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
|
btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]);
|
||||||
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
|
btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]);
|
||||||
float result[6];
|
float result[6];
|
||||||
|
|
||||||
solveImatrix(rhs_top, rhs_bot, result);
|
solveImatrix(rhs_top, rhs_bot, result);
|
||||||
@@ -1811,12 +1391,18 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// now do the loop over the m_links
|
// now do the loop over the m_links
|
||||||
for (int i = 0; i < num_links; ++i) {
|
for (int i = 0; i < num_links; ++i)
|
||||||
|
{
|
||||||
|
//acceleration of the child link = acceleration of the parent transformed into child frame +
|
||||||
|
// + coriolis acceleration
|
||||||
|
// + joint acceleration
|
||||||
const int parent = m_links[i].m_parent;
|
const int parent = m_links[i].m_parent;
|
||||||
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||||
accel_top[parent+1], accel_bottom[parent+1],
|
accel_top[parent+1], accel_bottom[parent+1],
|
||||||
accel_top[i+1], accel_bottom[i+1]);
|
accel_top[i+1], accel_bottom[i+1]);
|
||||||
joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
|
|
||||||
|
|
||||||
|
joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
|
||||||
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
|
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
|
||||||
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
|
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
|
||||||
}
|
}
|
||||||
@@ -1851,6 +1437,8 @@ void btMultiBody::stepVelocities(btScalar dt,
|
|||||||
/////////////////
|
/////////////////
|
||||||
|
|
||||||
// Final step: add the accelerations (times dt) to the velocities.
|
// Final step: add the accelerations (times dt) to the velocities.
|
||||||
|
//todo: do we already need to update the velocities, or can we move this into the constraint solver?
|
||||||
|
//the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach
|
||||||
applyDeltaVee(output, dt);
|
applyDeltaVee(output, dt);
|
||||||
|
|
||||||
|
|
||||||
@@ -1904,7 +1492,6 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
|
|||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
|
void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
|
||||||
{
|
{
|
||||||
int num_links = getNumLinks();
|
int num_links = getNumLinks();
|
||||||
@@ -1944,7 +1531,6 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
|
|||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
|
void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
|
||||||
{
|
{
|
||||||
@@ -1961,193 +1547,12 @@ void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, in
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
|
void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
|
||||||
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
|
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
|
||||||
{
|
{
|
||||||
// Temporary matrices/vectors -- use scratch space from caller
|
// Temporary matrices/vectors -- use scratch space from caller
|
||||||
// so that we don't have to keep reallocating every frame
|
// so that we don't have to keep reallocating every frame
|
||||||
|
|
||||||
int num_links = getNumLinks();
|
|
||||||
int m_dofCount = getNumDofs();
|
|
||||||
scratch_r.resize(m_dofCount);
|
|
||||||
scratch_v.resize(4*num_links + 4);
|
|
||||||
|
|
||||||
btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
|
|
||||||
btVector3 * v_ptr = &scratch_v[0];
|
|
||||||
|
|
||||||
// zhat_i^A (scratch space)
|
|
||||||
btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
|
|
||||||
btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
|
|
||||||
|
|
||||||
// rot_from_parent (cached from calcAccelerations)
|
|
||||||
const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
|
|
||||||
|
|
||||||
// hhat (cached), accel (scratch)
|
|
||||||
// hhat is NOT stored for the base (but ahat is)
|
|
||||||
const btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0;
|
|
||||||
const btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0;
|
|
||||||
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
|
|
||||||
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
|
||||||
|
|
||||||
// Y_i (scratch), invD_i (cached)
|
|
||||||
const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
|
|
||||||
btScalar * Y = r_ptr;
|
|
||||||
////////////////
|
|
||||||
|
|
||||||
// First 'upward' loop.
|
|
||||||
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
|
||||||
|
|
||||||
btVector3 input_force(force[3],force[4],force[5]);
|
|
||||||
btVector3 input_torque(force[0],force[1],force[2]);
|
|
||||||
|
|
||||||
// Fill in zero_acc
|
|
||||||
// -- set to force/torque on the base, zero otherwise
|
|
||||||
if (m_fixedBase)
|
|
||||||
{
|
|
||||||
zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0);
|
|
||||||
} else
|
|
||||||
{
|
|
||||||
zeroAccForce[0] = - (rot_from_parent[0] * input_force);
|
|
||||||
zeroAccTorque[0] = - (rot_from_parent[0] * input_torque);
|
|
||||||
}
|
|
||||||
for (int i = 0; i < num_links; ++i)
|
|
||||||
{
|
|
||||||
zeroAccForce[i+1] = zeroAccTorque[i+1] = btVector3(0,0,0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 'Downward' loop.
|
|
||||||
// (part of TreeForwardDynamics in Mirtich.)
|
|
||||||
for (int i = num_links - 1; i >= 0; --i)
|
|
||||||
{
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
//?? btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]);
|
|
||||||
|
|
||||||
Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
|
|
||||||
- SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
|
|
||||||
;
|
|
||||||
}
|
|
||||||
|
|
||||||
btScalar aa = Y[i];
|
|
||||||
const int parent = m_links[i].m_parent;
|
|
||||||
|
|
||||||
btVector3 in_top, in_bottom, out_top, out_bottom;
|
|
||||||
const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
|
||||||
|
|
||||||
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
invD_times_Y[dof] = 0.f;
|
|
||||||
|
|
||||||
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
|
||||||
{
|
|
||||||
invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Zp += pXi * (Zi + hi*Yi/Di)
|
|
||||||
in_top = zeroAccForce[i+1];
|
|
||||||
in_bottom = zeroAccTorque[i+1];
|
|
||||||
|
|
||||||
//unroll the loop?
|
|
||||||
for(int row = 0; row < 3; ++row)
|
|
||||||
{
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
|
||||||
const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
|
||||||
|
|
||||||
in_top[row] += h_t[row] * invD_times_Y[dof];
|
|
||||||
in_bottom[row] += h_b[row] * invD_times_Y[dof];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
||||||
in_top, in_bottom, out_top, out_bottom);
|
|
||||||
zeroAccForce[parent+1] += out_top;
|
|
||||||
zeroAccTorque[parent+1] += out_bottom;
|
|
||||||
}
|
|
||||||
|
|
||||||
// ptr to the joint accel part of the output
|
|
||||||
btScalar * joint_accel = output + 6;
|
|
||||||
|
|
||||||
|
|
||||||
// Second 'upward' loop
|
|
||||||
// (part of TreeForwardDynamics in Mirtich)
|
|
||||||
|
|
||||||
if (m_fixedBase)
|
|
||||||
{
|
|
||||||
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
|
|
||||||
btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
|
|
||||||
float result[6];
|
|
||||||
|
|
||||||
solveImatrix(rhs_top, rhs_bot, result);
|
|
||||||
for (int i = 0; i < 3; ++i) {
|
|
||||||
accel_top[0][i] = -result[i];
|
|
||||||
accel_bottom[0][i] = -result[i+3];
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
static btScalar Y_minus_hT_a[6]; //it's dofx1 for each body so a single 6x1 temp is enough
|
|
||||||
// now do the loop over the m_links
|
|
||||||
for (int i = 0; i < num_links; ++i) {
|
|
||||||
const int parent = m_links[i].m_parent;
|
|
||||||
|
|
||||||
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
|
||||||
accel_top[parent+1], accel_bottom[parent+1],
|
|
||||||
accel_top[i+1], accel_bottom[i+1]);
|
|
||||||
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
|
||||||
const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
|
||||||
|
|
||||||
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
|
||||||
mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
|
|
||||||
|
|
||||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
|
||||||
{
|
|
||||||
accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof);
|
|
||||||
accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// transform base accelerations back to the world frame.
|
|
||||||
btVector3 omegadot_out;
|
|
||||||
omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
|
|
||||||
output[0] = omegadot_out[0];
|
|
||||||
output[1] = omegadot_out[1];
|
|
||||||
output[2] = omegadot_out[2];
|
|
||||||
|
|
||||||
btVector3 vdot_out;
|
|
||||||
vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
|
|
||||||
|
|
||||||
output[3] = vdot_out[0];
|
|
||||||
output[4] = vdot_out[1];
|
|
||||||
output[5] = vdot_out[2];
|
|
||||||
|
|
||||||
/////////////////
|
|
||||||
//printf("delta = [");
|
|
||||||
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
|
||||||
// printf("%.2f ", output[dof]);
|
|
||||||
//printf("]\n");
|
|
||||||
/////////////////
|
|
||||||
}
|
|
||||||
#else //i.e. TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
|
|
||||||
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
|
|
||||||
{
|
|
||||||
// Temporary matrices/vectors -- use scratch space from caller
|
|
||||||
// so that we don't have to keep reallocating every frame
|
|
||||||
|
|
||||||
int num_links = getNumLinks();
|
int num_links = getNumLinks();
|
||||||
scratch_r.resize(m_dofCount);
|
scratch_r.resize(m_dofCount);
|
||||||
@@ -2303,7 +1708,6 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
|||||||
//printf("]\n");
|
//printf("]\n");
|
||||||
/////////////////
|
/////////////////
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
|
void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
|
||||||
@@ -2595,7 +1999,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
|||||||
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
|
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
|
||||||
case btMultibodyLink::ePlanar:
|
case btMultibodyLink::ePlanar:
|
||||||
{
|
{
|
||||||
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
||||||
@@ -2607,7 +2010,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
|||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
default:
|
default:
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
@@ -2722,7 +2124,6 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
|
|||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
|
||||||
case btMultibodyLink::ePlanar:
|
case btMultibodyLink::ePlanar:
|
||||||
{
|
{
|
||||||
results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
|
results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
|
||||||
@@ -2731,7 +2132,6 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
|
|||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
default:
|
default:
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
@@ -2894,3 +2294,52 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
|
|||||||
wakeUp();
|
wakeUp();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
|
||||||
|
{
|
||||||
|
|
||||||
|
int num_links = getNumLinks();
|
||||||
|
|
||||||
|
// Cached 3x3 rotation matrices from parent frame to this frame.
|
||||||
|
btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
|
||||||
|
|
||||||
|
rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
|
||||||
|
|
||||||
|
for (int i = 0; i < num_links; ++i)
|
||||||
|
{
|
||||||
|
const int parent = m_links[i].m_parent;
|
||||||
|
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
|
||||||
|
}
|
||||||
|
|
||||||
|
int nLinks = getNumLinks();
|
||||||
|
///base + num m_links
|
||||||
|
world_to_local.resize(nLinks+1);
|
||||||
|
local_origin.resize(nLinks+1);
|
||||||
|
|
||||||
|
world_to_local[0] = getWorldToBaseRot();
|
||||||
|
local_origin[0] = getBasePos();
|
||||||
|
|
||||||
|
for (int k=0;k<getNumLinks();k++)
|
||||||
|
{
|
||||||
|
const int parent = getParent(k);
|
||||||
|
world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
|
||||||
|
local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int link=0;link<getNumLinks();link++)
|
||||||
|
{
|
||||||
|
int index = link+1;
|
||||||
|
|
||||||
|
btVector3 posr = local_origin[index];
|
||||||
|
btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
|
||||||
|
btTransform tr;
|
||||||
|
tr.setIdentity();
|
||||||
|
tr.setOrigin(posr);
|
||||||
|
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||||
|
getLink(link).m_cachedWorldTransform = tr;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -95,7 +95,6 @@ public:
|
|||||||
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
|
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
|
||||||
bool disableParentCollision=false);
|
bool disableParentCollision=false);
|
||||||
|
|
||||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
|
||||||
void setupPlanar(int i, // 0 to num_links-1
|
void setupPlanar(int i, // 0 to num_links-1
|
||||||
btScalar mass,
|
btScalar mass,
|
||||||
const btVector3 &inertia,
|
const btVector3 &inertia,
|
||||||
@@ -104,7 +103,6 @@ public:
|
|||||||
const btVector3 &rotationAxis,
|
const btVector3 &rotationAxis,
|
||||||
const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
|
const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
|
||||||
bool disableParentCollision=false);
|
bool disableParentCollision=false);
|
||||||
#endif
|
|
||||||
|
|
||||||
const btMultibodyLink& getLink(int index) const
|
const btMultibodyLink& getLink(int index) const
|
||||||
{
|
{
|
||||||
@@ -151,6 +149,7 @@ public:
|
|||||||
const btVector3 & getLinkInertia(int i) const;
|
const btVector3 & getLinkInertia(int i) const;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//
|
//
|
||||||
// change mass (incomplete: can only change base mass and inertia at present)
|
// change mass (incomplete: can only change base mass and inertia at present)
|
||||||
//
|
//
|
||||||
@@ -185,6 +184,15 @@ public:
|
|||||||
setWorldToBaseRot(tr.getRotation().inverse());
|
setWorldToBaseRot(tr.getRotation().inverse());
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
btTransform getBaseWorldTransform() const
|
||||||
|
{
|
||||||
|
btTransform tr;
|
||||||
|
tr.setOrigin(getBasePos());
|
||||||
|
tr.setRotation(getWorldToBaseRot().inverse());
|
||||||
|
return tr;
|
||||||
|
}
|
||||||
|
|
||||||
void setBaseVel(const btVector3 &vel)
|
void setBaseVel(const btVector3 &vel)
|
||||||
{
|
{
|
||||||
|
|
||||||
@@ -217,6 +225,8 @@ public:
|
|||||||
void setJointPosMultiDof(int i, btScalar *q);
|
void setJointPosMultiDof(int i, btScalar *q);
|
||||||
void setJointVelMultiDof(int i, btScalar *qdot);
|
void setJointVelMultiDof(int i, btScalar *qdot);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//
|
//
|
||||||
// direct access to velocities as a vector of 6 + num_links elements.
|
// direct access to velocities as a vector of 6 + num_links elements.
|
||||||
// (omega first, then v, then joint velocities.)
|
// (omega first, then v, then joint velocities.)
|
||||||
@@ -389,6 +399,8 @@ public:
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// timestep the positions (given current velocities).
|
// timestep the positions (given current velocities).
|
||||||
void stepPositions(btScalar dt);
|
void stepPositions(btScalar dt);
|
||||||
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
|
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
|
||||||
@@ -536,6 +548,14 @@ public:
|
|||||||
__posUpdated = updated;
|
__posUpdated = updated;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//internalNeedsJointFeedback is for internal use only
|
||||||
|
bool internalNeedsJointFeedback() const
|
||||||
|
{
|
||||||
|
return m_internalNeedsJointFeedback;
|
||||||
|
}
|
||||||
|
void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
btMultiBody(const btMultiBody &); // not implemented
|
btMultiBody(const btMultiBody &); // not implemented
|
||||||
void operator=(const btMultiBody &); // not implemented
|
void operator=(const btMultiBody &); // not implemented
|
||||||
@@ -543,9 +563,7 @@ private:
|
|||||||
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
|
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
|
||||||
|
|
||||||
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
|
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
|
||||||
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
|
void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
|
||||||
#endif
|
|
||||||
|
|
||||||
void updateLinksDofOffsets()
|
void updateLinksDofOffsets()
|
||||||
{
|
{
|
||||||
@@ -560,6 +578,7 @@ private:
|
|||||||
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
|
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
btMultiBodyLinkCollider* m_baseCollider;//can be NULL
|
btMultiBodyLinkCollider* m_baseCollider;//can be NULL
|
||||||
@@ -622,6 +641,9 @@ private:
|
|||||||
bool __posUpdated;
|
bool __posUpdated;
|
||||||
int m_dofCount, m_posVarCnt;
|
int m_dofCount, m_posVarCnt;
|
||||||
bool m_useRK4, m_useGlobalVelocities;
|
bool m_useRK4, m_useGlobalVelocities;
|
||||||
|
|
||||||
|
///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
|
||||||
|
bool m_internalNeedsJointFeedback;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -93,6 +93,14 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
|
|||||||
|
|
||||||
if (multiBodyA)
|
if (multiBodyA)
|
||||||
{
|
{
|
||||||
|
if (solverConstraint.m_linkA<0)
|
||||||
|
{
|
||||||
|
rel_pos1 = posAworld - multiBodyA->getBasePos();
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
|
||||||
|
}
|
||||||
|
|
||||||
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||||
|
|
||||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||||
@@ -136,8 +144,12 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
|
|||||||
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||||
else
|
else
|
||||||
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||||
|
|
||||||
|
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
|
||||||
|
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||||
|
solverConstraint.m_contactNormal1 = contactNormalOnB;
|
||||||
}
|
}
|
||||||
else if(rb0)
|
else //if(rb0)
|
||||||
{
|
{
|
||||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
|
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
|
||||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||||
@@ -147,6 +159,14 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
|
|||||||
|
|
||||||
if (multiBodyB)
|
if (multiBodyB)
|
||||||
{
|
{
|
||||||
|
if (solverConstraint.m_linkB<0)
|
||||||
|
{
|
||||||
|
rel_pos2 = posBworld - multiBodyB->getBasePos();
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
|
||||||
|
}
|
||||||
|
|
||||||
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||||
|
|
||||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||||
@@ -186,8 +206,12 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
|
|||||||
else
|
else
|
||||||
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
|
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
|
||||||
|
|
||||||
|
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
|
||||||
|
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||||
|
solverConstraint.m_contactNormal2 = -contactNormalOnB;
|
||||||
|
|
||||||
}
|
}
|
||||||
else if(rb1)
|
else //if(rb1)
|
||||||
{
|
{
|
||||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
|
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
|
||||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||||
|
|||||||
@@ -13,6 +13,7 @@ subject to the following restrictions:
|
|||||||
3. This notice may not be removed or altered from any source distribution.
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
#include "btMultiBodyConstraintSolver.h"
|
#include "btMultiBodyConstraintSolver.h"
|
||||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||||
#include "btMultiBodyLinkCollider.h"
|
#include "btMultiBodyLinkCollider.h"
|
||||||
@@ -165,12 +166,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
|
|||||||
if (c.m_multiBodyA)
|
if (c.m_multiBodyA)
|
||||||
{
|
{
|
||||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
|
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
|
||||||
|
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||||
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
||||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||||
if(c.m_multiBodyA->isMultiDof())
|
if(c.m_multiBodyA->isMultiDof())
|
||||||
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||||
else
|
else
|
||||||
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||||
|
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||||
} else if(c.m_solverBodyIdA >= 0)
|
} else if(c.m_solverBodyIdA >= 0)
|
||||||
{
|
{
|
||||||
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||||
@@ -179,12 +182,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
|
|||||||
if (c.m_multiBodyB)
|
if (c.m_multiBodyB)
|
||||||
{
|
{
|
||||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
|
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
|
||||||
|
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||||
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
||||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||||
if(c.m_multiBodyB->isMultiDof())
|
if(c.m_multiBodyB->isMultiDof())
|
||||||
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||||
else
|
else
|
||||||
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||||
|
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||||
} else if(c.m_solverBodyIdB >= 0)
|
} else if(c.m_solverBodyIdB >= 0)
|
||||||
{
|
{
|
||||||
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||||
@@ -279,8 +284,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
|||||||
|
|
||||||
relaxation = 1.f;
|
relaxation = 1.f;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (multiBodyA)
|
if (multiBodyA)
|
||||||
{
|
{
|
||||||
|
if (solverConstraint.m_linkA<0)
|
||||||
|
{
|
||||||
|
rel_pos1 = pos1 - multiBodyA->getBasePos();
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
|
||||||
|
}
|
||||||
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||||
|
|
||||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||||
@@ -310,16 +325,30 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
|||||||
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
|
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
|
||||||
else
|
else
|
||||||
multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
|
multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
|
||||||
|
|
||||||
|
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
|
||||||
|
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||||
|
solverConstraint.m_contactNormal1 = contactNormal;
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
|
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
|
||||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
|
||||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||||
solverConstraint.m_contactNormal1 = contactNormal;
|
solverConstraint.m_contactNormal1 = contactNormal;
|
||||||
|
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (multiBodyB)
|
if (multiBodyB)
|
||||||
{
|
{
|
||||||
|
if (solverConstraint.m_linkB<0)
|
||||||
|
{
|
||||||
|
rel_pos2 = pos2 - multiBodyB->getBasePos();
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
|
||||||
|
}
|
||||||
|
|
||||||
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||||
|
|
||||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||||
@@ -344,12 +373,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
|||||||
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
|
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
|
||||||
else
|
else
|
||||||
multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
|
multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
|
||||||
|
|
||||||
|
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
|
||||||
|
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||||
|
solverConstraint.m_contactNormal2 = -contactNormal;
|
||||||
|
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
|
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
|
||||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
|
||||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||||
solverConstraint.m_contactNormal2 = -contactNormal;
|
solverConstraint.m_contactNormal2 = -contactNormal;
|
||||||
|
|
||||||
|
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
@@ -577,6 +612,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
|
|||||||
{
|
{
|
||||||
BT_PROFILE("addMultiBodyFrictionConstraint");
|
BT_PROFILE("addMultiBodyFrictionConstraint");
|
||||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
|
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
|
||||||
|
solverConstraint.m_useJointForce = false;
|
||||||
solverConstraint.m_frictionIndex = frictionIndex;
|
solverConstraint.m_frictionIndex = frictionIndex;
|
||||||
bool isFriction = true;
|
bool isFriction = true;
|
||||||
|
|
||||||
@@ -644,6 +680,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
|
|||||||
int frictionIndex = m_multiBodyNormalContactConstraints.size();
|
int frictionIndex = m_multiBodyNormalContactConstraints.size();
|
||||||
|
|
||||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
|
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
|
||||||
|
solverConstraint.m_useJointForce = false;
|
||||||
|
|
||||||
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
||||||
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
||||||
@@ -824,32 +861,267 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
|
|||||||
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
|
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
|
||||||
{
|
{
|
||||||
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
|
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
|
||||||
|
}
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
|
||||||
|
{
|
||||||
|
if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
|
||||||
|
{
|
||||||
|
//todo: get rid of those temporary memory allocations for the joint feedback
|
||||||
|
btAlignedObjectArray<btScalar> forceVector;
|
||||||
|
int numDofsPlusBase = 6+mb->getNumDofs();
|
||||||
|
forceVector.resize(numDofsPlusBase);
|
||||||
|
for (int i=0;i<numDofsPlusBase;i++)
|
||||||
|
{
|
||||||
|
forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
|
||||||
|
}
|
||||||
|
btAlignedObjectArray<btScalar> output;
|
||||||
|
output.resize(numDofsPlusBase);
|
||||||
|
bool applyJointFeedback = true;
|
||||||
|
mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include "Bullet3Common/b3Logging.h"
|
||||||
|
void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
|
||||||
|
{
|
||||||
|
#if 1
|
||||||
|
|
||||||
|
//bod->addBaseForce(m_gravity * bod->getBaseMass());
|
||||||
|
//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
|
||||||
|
|
||||||
|
|
||||||
|
if (c.m_multiBodyA)
|
||||||
|
{
|
||||||
|
btScalar ai = c.m_appliedImpulse;
|
||||||
|
|
||||||
|
if(c.m_multiBodyA->isMultiDof())
|
||||||
|
{
|
||||||
|
if (c.m_useJointForce)
|
||||||
|
{
|
||||||
|
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||||
|
//c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
|
||||||
|
} else
|
||||||
|
//if (c.m_multiBodyA->getCompanionId()>=0)
|
||||||
|
{
|
||||||
|
c.m_multiBodyA->setCompanionId(-1);
|
||||||
|
btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
|
||||||
|
btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
|
||||||
|
if (c.m_linkA<0)
|
||||||
|
{
|
||||||
|
c.m_multiBodyA->addBaseForce(force);
|
||||||
|
c.m_multiBodyA->addBaseTorque(torque);
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
if (c.m_useJointForce)
|
||||||
|
{
|
||||||
|
c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
c.m_multiBodyA->addLinkForce(c.m_linkA,force);
|
||||||
|
//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
|
||||||
|
c.m_multiBodyA->addLinkTorque(c.m_linkA,torque);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (c.m_multiBodyB)
|
||||||
|
{
|
||||||
|
if(c.m_multiBodyB->isMultiDof())
|
||||||
|
{
|
||||||
|
if (c.m_useJointForce)
|
||||||
|
{
|
||||||
|
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||||
|
} else
|
||||||
|
|
||||||
|
//if (c.m_multiBodyB->getCompanionId()>=0)
|
||||||
|
{
|
||||||
|
c.m_multiBodyB->setCompanionId(-1);
|
||||||
|
btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
|
||||||
|
btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
|
||||||
|
if (c.m_linkB<0)
|
||||||
|
{
|
||||||
|
c.m_multiBodyB->addBaseForce(force);
|
||||||
|
c.m_multiBodyB->addBaseTorque(torque);
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
if (!c.m_useJointForce)
|
||||||
|
{
|
||||||
|
c.m_multiBodyB->addLinkForce(c.m_linkB,force);
|
||||||
|
//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
|
||||||
|
c.m_multiBodyB->addLinkTorque(c.m_linkB,torque);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
//c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
|
||||||
|
if (c.m_multiBodyA)
|
||||||
|
{
|
||||||
|
|
||||||
|
if(c.m_multiBodyA->isMultiDof())
|
||||||
|
{
|
||||||
|
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (c.m_multiBodyB)
|
||||||
|
{
|
||||||
|
if(c.m_multiBodyB->isMultiDof())
|
||||||
|
{
|
||||||
|
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
|
btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
|
||||||
{
|
{
|
||||||
|
BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
|
||||||
int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
|
int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
|
||||||
int j;
|
|
||||||
|
#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||||
|
|
||||||
|
//write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
|
||||||
|
//or as applied force, so we can measure the joint reaction forces easier
|
||||||
|
for (int i=0;i<numPoolConstraints;i++)
|
||||||
|
{
|
||||||
|
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
|
||||||
|
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
|
||||||
|
|
||||||
|
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
|
||||||
|
|
||||||
|
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||||
|
{
|
||||||
|
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
|
||||||
|
{
|
||||||
|
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
|
||||||
|
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
|
||||||
|
}
|
||||||
|
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||||
|
|
||||||
|
|
||||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||||
{
|
{
|
||||||
for (j=0;j<numPoolConstraints;j++)
|
BT_PROFILE("warm starting write back");
|
||||||
|
for (int j=0;j<numPoolConstraints;j++)
|
||||||
{
|
{
|
||||||
const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
|
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
|
||||||
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
|
btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
|
||||||
btAssert(pt);
|
btAssert(pt);
|
||||||
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
|
pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
|
||||||
|
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
|
||||||
|
|
||||||
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse;
|
|
||||||
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
|
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
|
||||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||||
{
|
{
|
||||||
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse;
|
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
|
||||||
}
|
}
|
||||||
//do a callback here?
|
//do a callback here?
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#if 0
|
||||||
|
//multibody joint feedback
|
||||||
|
{
|
||||||
|
BT_PROFILE("multi body joint feedback");
|
||||||
|
for (int j=0;j<numPoolConstraints;j++)
|
||||||
|
{
|
||||||
|
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
|
||||||
|
|
||||||
|
//apply the joint feedback into all links of the btMultiBody
|
||||||
|
//todo: double-check the signs of the applied impulse
|
||||||
|
|
||||||
|
if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
|
||||||
|
{
|
||||||
|
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||||
|
}
|
||||||
|
if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
|
||||||
|
{
|
||||||
|
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
|
||||||
|
}
|
||||||
|
#if 0
|
||||||
|
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
|
||||||
|
{
|
||||||
|
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||||
|
|
||||||
|
}
|
||||||
|
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
|
||||||
|
{
|
||||||
|
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||||
|
{
|
||||||
|
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
|
||||||
|
{
|
||||||
|
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
|
||||||
|
{
|
||||||
|
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
|
||||||
|
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
|
||||||
|
{
|
||||||
|
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
|
||||||
|
if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
|
||||||
|
{
|
||||||
|
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||||
|
}
|
||||||
|
if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
|
||||||
|
{
|
||||||
|
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
numPoolConstraints = m_multiBodyNonContactConstraints.size();
|
numPoolConstraints = m_multiBodyNonContactConstraints.size();
|
||||||
|
|
||||||
@@ -878,7 +1150,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
|
|||||||
|
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
|
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -19,6 +19,7 @@ subject to the following restrictions:
|
|||||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||||
#include "btMultiBodySolverConstraint.h"
|
#include "btMultiBodySolverConstraint.h"
|
||||||
|
|
||||||
|
//#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||||
|
|
||||||
class btMultiBody;
|
class btMultiBody;
|
||||||
|
|
||||||
@@ -66,7 +67,7 @@ protected:
|
|||||||
|
|
||||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||||
void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
|
void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
|
||||||
|
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
|
||||||
public:
|
public:
|
||||||
|
|
||||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
|
|||||||
@@ -394,8 +394,20 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
|
|||||||
delete m_solverMultiBodyIslandCallback;
|
delete m_solverMultiBodyIslandCallback;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void btMultiBodyDynamicsWorld::forwardKinematics()
|
||||||
|
{
|
||||||
|
btAlignedObjectArray<btQuaternion> world_to_local;
|
||||||
|
btAlignedObjectArray<btVector3> local_origin;
|
||||||
|
|
||||||
|
for (int b=0;b<m_multiBodies.size();b++)
|
||||||
|
{
|
||||||
|
btMultiBody* bod = m_multiBodies[b];
|
||||||
|
bod->forwardKinematics(world_to_local,local_origin);
|
||||||
|
}
|
||||||
|
}
|
||||||
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||||
{
|
{
|
||||||
|
forwardKinematics();
|
||||||
|
|
||||||
btAlignedObjectArray<btScalar> scratch_r;
|
btAlignedObjectArray<btScalar> scratch_r;
|
||||||
btAlignedObjectArray<btVector3> scratch_v;
|
btAlignedObjectArray<btVector3> scratch_v;
|
||||||
@@ -431,7 +443,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
|||||||
|
|
||||||
|
|
||||||
{
|
{
|
||||||
BT_PROFILE("btMultiBody addForce and stepVelocities");
|
BT_PROFILE("btMultiBody addForce");
|
||||||
for (int i=0;i<this->m_multiBodies.size();i++)
|
for (int i=0;i<this->m_multiBodies.size();i++)
|
||||||
{
|
{
|
||||||
btMultiBody* bod = m_multiBodies[i];
|
btMultiBody* bod = m_multiBodies[i];
|
||||||
@@ -461,7 +473,36 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
|||||||
{
|
{
|
||||||
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
|
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
|
||||||
}
|
}
|
||||||
|
}//if (!isSleeping)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
{
|
||||||
|
BT_PROFILE("btMultiBody stepVelocities");
|
||||||
|
for (int i=0;i<this->m_multiBodies.size();i++)
|
||||||
|
{
|
||||||
|
btMultiBody* bod = m_multiBodies[i];
|
||||||
|
|
||||||
|
bool isSleeping = false;
|
||||||
|
|
||||||
|
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
|
||||||
|
{
|
||||||
|
isSleeping = true;
|
||||||
|
}
|
||||||
|
for (int b=0;b<bod->getNumLinks();b++)
|
||||||
|
{
|
||||||
|
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
|
||||||
|
isSleeping = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!isSleeping)
|
||||||
|
{
|
||||||
|
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
|
||||||
|
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
|
||||||
|
scratch_v.resize(bod->getNumLinks()+1);
|
||||||
|
scratch_m.resize(bod->getNumLinks()+1);
|
||||||
bool doNotUpdatePos = false;
|
bool doNotUpdatePos = false;
|
||||||
|
|
||||||
if(bod->isMultiDof())
|
if(bod->isMultiDof())
|
||||||
@@ -639,6 +680,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
|||||||
|
|
||||||
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
|
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
|
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
|
||||||
@@ -779,8 +822,8 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
|
|||||||
{
|
{
|
||||||
BT_PROFILE("btMultiBody debugDrawWorld");
|
BT_PROFILE("btMultiBody debugDrawWorld");
|
||||||
|
|
||||||
btAlignedObjectArray<btQuaternion> world_to_local;
|
btAlignedObjectArray<btQuaternion> world_to_local1;
|
||||||
btAlignedObjectArray<btVector3> local_origin;
|
btAlignedObjectArray<btVector3> local_origin1;
|
||||||
|
|
||||||
for (int c=0;c<m_multiBodyConstraints.size();c++)
|
for (int c=0;c<m_multiBodyConstraints.size();c++)
|
||||||
{
|
{
|
||||||
@@ -791,52 +834,40 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
|
|||||||
for (int b = 0; b<m_multiBodies.size(); b++)
|
for (int b = 0; b<m_multiBodies.size(); b++)
|
||||||
{
|
{
|
||||||
btMultiBody* bod = m_multiBodies[b];
|
btMultiBody* bod = m_multiBodies[b];
|
||||||
|
bod->forwardKinematics(world_to_local1,local_origin1);
|
||||||
|
|
||||||
|
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
|
||||||
|
|
||||||
int nLinks = bod->getNumLinks();
|
int nLinks = bod->getNumLinks();
|
||||||
|
|
||||||
///base + num m_links
|
|
||||||
world_to_local.resize(nLinks + 1);
|
|
||||||
local_origin.resize(nLinks + 1);
|
|
||||||
|
|
||||||
|
|
||||||
world_to_local[0] = bod->getWorldToBaseRot();
|
|
||||||
local_origin[0] = bod->getBasePos();
|
|
||||||
|
|
||||||
|
|
||||||
{
|
|
||||||
btVector3 posr = local_origin[0];
|
|
||||||
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
|
||||||
btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
|
|
||||||
btTransform tr;
|
|
||||||
tr.setIdentity();
|
|
||||||
tr.setOrigin(posr);
|
|
||||||
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
|
|
||||||
|
|
||||||
getDebugDrawer()->drawTransform(tr, 0.1);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int k = 0; k<bod->getNumLinks(); k++)
|
|
||||||
{
|
|
||||||
const int parent = bod->getParent(k);
|
|
||||||
world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
|
|
||||||
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
for (int m = 0; m<bod->getNumLinks(); m++)
|
for (int m = 0; m<bod->getNumLinks(); m++)
|
||||||
{
|
{
|
||||||
int link = m;
|
int link = m;
|
||||||
int index = link + 1;
|
|
||||||
|
|
||||||
btVector3 posr = local_origin[index];
|
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
|
||||||
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
|
||||||
btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
|
|
||||||
btTransform tr;
|
|
||||||
tr.setIdentity();
|
|
||||||
tr.setOrigin(posr);
|
|
||||||
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
|
|
||||||
|
|
||||||
getDebugDrawer()->drawTransform(tr, 0.1);
|
getDebugDrawer()->drawTransform(tr, 0.1);
|
||||||
|
|
||||||
|
//draw the joint axis
|
||||||
|
if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
|
||||||
|
{
|
||||||
|
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
|
||||||
|
|
||||||
|
btVector4 color(0,0,0,1);//1,1,1);
|
||||||
|
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
|
||||||
|
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
|
||||||
|
getDebugDrawer()->drawLine(from,to,color);
|
||||||
|
}
|
||||||
|
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
|
||||||
|
{
|
||||||
|
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
|
||||||
|
|
||||||
|
btVector4 color(0,0,0,1);//1,1,1);
|
||||||
|
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
|
||||||
|
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
|
||||||
|
getDebugDrawer()->drawLine(from,to,color);
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -49,6 +49,16 @@ public:
|
|||||||
|
|
||||||
virtual void removeMultiBody(btMultiBody* body);
|
virtual void removeMultiBody(btMultiBody* body);
|
||||||
|
|
||||||
|
virtual int getNumMultibodies() const
|
||||||
|
{
|
||||||
|
return m_multiBodies.size();
|
||||||
|
}
|
||||||
|
|
||||||
|
btMultiBody* getMultiBody(int mbIndex)
|
||||||
|
{
|
||||||
|
return m_multiBodies[mbIndex];
|
||||||
|
}
|
||||||
|
|
||||||
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
|
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
|
||||||
|
|
||||||
virtual int getNumMultiBodyConstraints() const
|
virtual int getNumMultiBodyConstraints() const
|
||||||
@@ -73,5 +83,8 @@ public:
|
|||||||
virtual void debugDrawWorld();
|
virtual void debugDrawWorld();
|
||||||
|
|
||||||
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
|
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
|
||||||
|
|
||||||
|
void forwardKinematics();
|
||||||
|
|
||||||
};
|
};
|
||||||
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||||
|
|||||||
28
src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
Normal file
28
src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
Normal file
@@ -0,0 +1,28 @@
|
|||||||
|
/*
|
||||||
|
Copyright (c) 2015 Google Inc.
|
||||||
|
|
||||||
|
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 BT_MULTIBODY_JOINT_FEEDBACK_H
|
||||||
|
#define BT_MULTIBODY_JOINT_FEEDBACK_H
|
||||||
|
|
||||||
|
#include "LinearMath/btSpatialAlgebra.h"
|
||||||
|
|
||||||
|
struct btMultiBodyJointFeedback
|
||||||
|
{
|
||||||
|
btSpatialForceVector m_reactionForces;
|
||||||
|
btSymmetricSpatialDyad m_spatialInertia;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H
|
||||||
@@ -21,9 +21,10 @@ subject to the following restrictions:
|
|||||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
|
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
|
||||||
//:btMultiBodyConstraint(body,0,link,-1,2,true),
|
//:btMultiBodyConstraint(body,0,link,-1,2,true),
|
||||||
:btMultiBodyConstraint(body,body,link,link,2,true),
|
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
|
||||||
m_lowerBound(lower),
|
m_lowerBound(lower),
|
||||||
m_upperBound(upper)
|
m_upperBound(upper)
|
||||||
{
|
{
|
||||||
@@ -43,7 +44,6 @@ void btMultiBodyJointLimitConstraint::finalizeMultiDof()
|
|||||||
jacobianA(0)[offset] = 1;
|
jacobianA(0)[offset] = 1;
|
||||||
// row 1: the upper bound
|
// row 1: the upper bound
|
||||||
//jacobianA(1)[offset] = -1;
|
//jacobianA(1)[offset] = -1;
|
||||||
|
|
||||||
jacobianB(1)[offset] = -1;
|
jacobianB(1)[offset] = -1;
|
||||||
|
|
||||||
m_numDofsFinalized = m_jacSizeBoth;
|
m_numDofsFinalized = m_jacSizeBoth;
|
||||||
@@ -92,6 +92,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
|
|||||||
btMultiBodyJacobianData& data,
|
btMultiBodyJacobianData& data,
|
||||||
const btContactSolverInfo& infoGlobal)
|
const btContactSolverInfo& infoGlobal)
|
||||||
{
|
{
|
||||||
|
|
||||||
// only positions need to be updated -- data.m_jacobians and force
|
// only positions need to be updated -- data.m_jacobians and force
|
||||||
// directions were set in the ctor and never change.
|
// directions were set in the ctor and never change.
|
||||||
|
|
||||||
@@ -109,6 +110,9 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
|
|||||||
|
|
||||||
for (int row=0;row<getNumRows();row++)
|
for (int row=0;row<getNumRows();row++)
|
||||||
{
|
{
|
||||||
|
|
||||||
|
btScalar direction = row? -1 : 1;
|
||||||
|
|
||||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||||
constraintRow.m_multiBodyA = m_bodyA;
|
constraintRow.m_multiBodyA = m_bodyA;
|
||||||
constraintRow.m_multiBodyB = m_bodyB;
|
constraintRow.m_multiBodyB = m_bodyB;
|
||||||
@@ -116,6 +120,42 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
|
|||||||
const btVector3 dummy(0, 0, 0);
|
const btVector3 dummy(0, 0, 0);
|
||||||
|
|
||||||
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
|
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
|
||||||
|
|
||||||
|
if (m_bodyA->isMultiDof())
|
||||||
|
{
|
||||||
|
constraintRow.m_useJointForce = false;
|
||||||
|
//expect either prismatic or revolute joint type for now
|
||||||
|
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
|
||||||
|
switch (m_bodyA->getLink(m_linkA).m_jointType)
|
||||||
|
{
|
||||||
|
case btMultibodyLink::eRevolute:
|
||||||
|
{
|
||||||
|
constraintRow.m_contactNormal1.setZero();
|
||||||
|
constraintRow.m_contactNormal2.setZero();
|
||||||
|
btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
|
||||||
|
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
|
||||||
|
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case btMultibodyLink::ePrismatic:
|
||||||
|
{
|
||||||
|
btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
|
||||||
|
constraintRow.m_contactNormal1=prismaticAxisInWorld;
|
||||||
|
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
|
||||||
|
constraintRow.m_relpos1CrossNormal.setZero();
|
||||||
|
constraintRow.m_relpos2CrossNormal.setZero();
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
{
|
||||||
|
btAssert(0);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
btScalar penetration = getPosition(row);
|
btScalar penetration = getPosition(row);
|
||||||
btScalar positionalError = 0.f;
|
btScalar positionalError = 0.f;
|
||||||
|
|||||||
@@ -22,11 +22,11 @@ subject to the following restrictions:
|
|||||||
|
|
||||||
|
|
||||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||||
:btMultiBodyConstraint(body,body,link,link,1,true),
|
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
|
||||||
m_desiredVelocity(desiredVelocity)
|
m_desiredVelocity(desiredVelocity)
|
||||||
{
|
{
|
||||||
|
|
||||||
|
int parent = body->getLink(link).m_parent;
|
||||||
m_maxAppliedImpulse = maxMotorImpulse;
|
m_maxAppliedImpulse = maxMotorImpulse;
|
||||||
// the data.m_jacobians never change, so may as well
|
// the data.m_jacobians never change, so may as well
|
||||||
// initialize them here
|
// initialize them here
|
||||||
@@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof()
|
|||||||
|
|
||||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||||
//:btMultiBodyConstraint(body,0,link,-1,1,true),
|
//:btMultiBodyConstraint(body,0,link,-1,1,true),
|
||||||
:btMultiBodyConstraint(body,body,link,link,1,true),
|
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
|
||||||
m_desiredVelocity(desiredVelocity)
|
m_desiredVelocity(desiredVelocity)
|
||||||
{
|
{
|
||||||
btAssert(linkDoF < body->getLink(link).m_dofCount);
|
btAssert(linkDoF < body->getLink(link).m_dofCount);
|
||||||
@@ -117,6 +117,42 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
|
|||||||
|
|
||||||
|
|
||||||
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
|
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
|
||||||
|
|
||||||
|
if (m_bodyA->isMultiDof())
|
||||||
|
{
|
||||||
|
constraintRow.m_useJointForce = false;
|
||||||
|
//expect either prismatic or revolute joint type for now
|
||||||
|
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
|
||||||
|
switch (m_bodyA->getLink(m_linkA).m_jointType)
|
||||||
|
{
|
||||||
|
case btMultibodyLink::eRevolute:
|
||||||
|
{
|
||||||
|
constraintRow.m_contactNormal1.setZero();
|
||||||
|
constraintRow.m_contactNormal2.setZero();
|
||||||
|
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
|
||||||
|
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
|
||||||
|
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case btMultibodyLink::ePrismatic:
|
||||||
|
{
|
||||||
|
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
|
||||||
|
constraintRow.m_contactNormal1=prismaticAxisInWorld;
|
||||||
|
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
|
||||||
|
constraintRow.m_relpos1CrossNormal.setZero();
|
||||||
|
constraintRow.m_relpos2CrossNormal.setZero();
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
{
|
||||||
|
btAssert(0);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -25,6 +25,7 @@ enum btMultiBodyLinkFlags
|
|||||||
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
|
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
|
||||||
};
|
};
|
||||||
|
|
||||||
|
//both defines are now permanently enabled
|
||||||
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||||
#define TEST_SPATIAL_ALGEBRA_LAYER
|
#define TEST_SPATIAL_ALGEBRA_LAYER
|
||||||
|
|
||||||
@@ -34,11 +35,9 @@ enum btMultiBodyLinkFlags
|
|||||||
|
|
||||||
//namespace {
|
//namespace {
|
||||||
|
|
||||||
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
|
|
||||||
#include "LinearMath/btSpatialAlgebra.h"
|
#include "LinearMath/btSpatialAlgebra.h"
|
||||||
|
|
||||||
#endif
|
|
||||||
//}
|
//}
|
||||||
|
|
||||||
//
|
//
|
||||||
@@ -64,18 +63,14 @@ struct btMultibodyLink
|
|||||||
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
|
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
|
||||||
btVector3 m_eVector;
|
btVector3 m_eVector;
|
||||||
|
|
||||||
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
|
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
|
||||||
#endif
|
|
||||||
|
|
||||||
enum eFeatherstoneJointType
|
enum eFeatherstoneJointType
|
||||||
{
|
{
|
||||||
eRevolute = 0,
|
eRevolute = 0,
|
||||||
ePrismatic = 1,
|
ePrismatic = 1,
|
||||||
eSpherical = 2,
|
eSpherical = 2,
|
||||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
|
||||||
ePlanar = 3,
|
ePlanar = 3,
|
||||||
#endif
|
|
||||||
eFixed = 4,
|
eFixed = 4,
|
||||||
eInvalid
|
eInvalid
|
||||||
};
|
};
|
||||||
@@ -95,16 +90,6 @@ struct btMultibodyLink
|
|||||||
// m_axesTop[1][2] = zero
|
// m_axesTop[1][2] = zero
|
||||||
// m_axesBottom[0] = zero
|
// m_axesBottom[0] = zero
|
||||||
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
|
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
|
||||||
#ifndef TEST_SPATIAL_ALGEBRA_LAYER
|
|
||||||
btVector3 m_axesTop[6];
|
|
||||||
btVector3 m_axesBottom[6];
|
|
||||||
void setAxisTop(int dof, const btVector3 &axis) { m_axesTop[dof] = axis; }
|
|
||||||
void setAxisBottom(int dof, const btVector3 &axis) { m_axesBottom[dof] = axis; }
|
|
||||||
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesTop[dof].setValue(x, y, z); }
|
|
||||||
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesBottom[dof].setValue(x, y, z); }
|
|
||||||
const btVector3 & getAxisTop(int dof) const { return m_axesTop[dof]; }
|
|
||||||
const btVector3 & getAxisBottom(int dof) const { return m_axesBottom[dof]; }
|
|
||||||
#else
|
|
||||||
btSpatialMotionVector m_axes[6];
|
btSpatialMotionVector m_axes[6];
|
||||||
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
|
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
|
||||||
void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
|
void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
|
||||||
@@ -112,7 +97,6 @@ struct btMultibodyLink
|
|||||||
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
|
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
|
||||||
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
|
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
|
||||||
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
|
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
|
||||||
#endif
|
|
||||||
|
|
||||||
int m_dofOffset, m_cfgOffset;
|
int m_dofOffset, m_cfgOffset;
|
||||||
|
|
||||||
@@ -123,15 +107,23 @@ struct btMultibodyLink
|
|||||||
btVector3 m_appliedTorque; // In WORLD frame
|
btVector3 m_appliedTorque; // In WORLD frame
|
||||||
|
|
||||||
btScalar m_jointPos[7];
|
btScalar m_jointPos[7];
|
||||||
btScalar m_jointTorque[6]; //TODO
|
|
||||||
|
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
|
||||||
|
//It gets set to zero after each internal stepSimulation call
|
||||||
|
btScalar m_jointTorque[6];
|
||||||
|
|
||||||
class btMultiBodyLinkCollider* m_collider;
|
class btMultiBodyLinkCollider* m_collider;
|
||||||
int m_flags;
|
int m_flags;
|
||||||
|
|
||||||
|
|
||||||
int m_dofCount, m_posVarCount; //redundant but handy
|
int m_dofCount, m_posVarCount; //redundant but handy
|
||||||
|
|
||||||
eFeatherstoneJointType m_jointType;
|
eFeatherstoneJointType m_jointType;
|
||||||
|
|
||||||
|
struct btMultiBodyJointFeedback* m_jointFeedback;
|
||||||
|
|
||||||
|
btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
|
||||||
|
|
||||||
// ctor: set some sensible defaults
|
// ctor: set some sensible defaults
|
||||||
btMultibodyLink()
|
btMultibodyLink()
|
||||||
: m_mass(1),
|
: m_mass(1),
|
||||||
@@ -142,8 +134,11 @@ struct btMultibodyLink
|
|||||||
m_flags(0),
|
m_flags(0),
|
||||||
m_dofCount(0),
|
m_dofCount(0),
|
||||||
m_posVarCount(0),
|
m_posVarCount(0),
|
||||||
m_jointType(btMultibodyLink::eInvalid)
|
m_jointType(btMultibodyLink::eInvalid),
|
||||||
|
m_jointFeedback(0)
|
||||||
|
|
||||||
{
|
{
|
||||||
|
|
||||||
m_inertiaLocal.setValue(1, 1, 1);
|
m_inertiaLocal.setValue(1, 1, 1);
|
||||||
setAxisTop(0, 0., 0., 0.);
|
setAxisTop(0, 0., 0., 0.);
|
||||||
setAxisBottom(0, 1., 0., 0.);
|
setAxisBottom(0, 1., 0., 0.);
|
||||||
@@ -156,6 +151,7 @@ struct btMultibodyLink
|
|||||||
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
|
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
|
||||||
m_jointPos[3] = 1.f; //"quat.w"
|
m_jointPos[3] = 1.f; //"quat.w"
|
||||||
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
|
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
|
||||||
|
m_cachedWorldTransform.setIdentity();
|
||||||
}
|
}
|
||||||
|
|
||||||
// routine to update m_cachedRotParentToThis and m_cachedRVector
|
// routine to update m_cachedRotParentToThis and m_cachedRVector
|
||||||
@@ -200,7 +196,6 @@ struct btMultibodyLink
|
|||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
|
||||||
case ePlanar:
|
case ePlanar:
|
||||||
{
|
{
|
||||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
|
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
|
||||||
@@ -208,7 +203,6 @@ struct btMultibodyLink
|
|||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
case eFixed:
|
case eFixed:
|
||||||
{
|
{
|
||||||
m_cachedRotParentToThis = m_zeroRotParentToThis;
|
m_cachedRotParentToThis = m_zeroRotParentToThis;
|
||||||
|
|||||||
@@ -108,6 +108,7 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
|
|||||||
|
|
||||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||||
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
|
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
|
||||||
|
constraintRow.m_useJointForce = false;
|
||||||
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
|
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
|
||||||
constraintRow.m_contactNormal1.setValue(0,0,0);
|
constraintRow.m_contactNormal1.setValue(0,0,0);
|
||||||
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
|
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
|
||||||
|
|||||||
@@ -28,7 +28,7 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
|
|||||||
{
|
{
|
||||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||||
|
|
||||||
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1)
|
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_useJointForce(false)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
|
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
|
||||||
@@ -73,6 +73,8 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
|
|||||||
btMultiBody* m_multiBodyB;
|
btMultiBody* m_multiBodyB;
|
||||||
int m_linkB;
|
int m_linkB;
|
||||||
|
|
||||||
|
bool m_useJointForce;//needed for write-back of joint versus link/base force/torque
|
||||||
|
|
||||||
enum btSolverConstraintType
|
enum btSolverConstraintType
|
||||||
{
|
{
|
||||||
BT_SOLVER_CONTACT_1D = 0,
|
BT_SOLVER_CONTACT_1D = 0,
|
||||||
|
|||||||
Reference in New Issue
Block a user