Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans
2016-08-27 10:37:48 -07:00
parent fb3f2d896c c741b17da8
commit 887e88a490
13 changed files with 496 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/MultiBody/MultiDofDemo.cpp
View File

@@ -11,6 +11,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
@@ -227,7 +228,7 @@ void MultiDofDemo::initPhysics()
startTransform.setOrigin(btVector3(
btScalar(0.0),
-0.95,
0.0,
btScalar(0.0)));
@@ -245,7 +246,8 @@ void MultiDofDemo::initPhysics()
btMatrix3x3 frameInB;
frameInA.setIdentity();
frameInB.setIdentity();
btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB);
btVector3 jointAxis(1.0,0.0,0.0);
btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
//btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB);
p2p->setMaxAppliedImpulse(2.0);
m_dynamicsWorld->addMultiBodyConstraint(p2p);
@@ -307,7 +309,7 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
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->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();

4
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -87,7 +87,7 @@ public:
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = true;
args.m_useMultiBody = false;
m_robotSim.loadFile(args, results);
}
@@ -152,7 +152,7 @@ public:
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.setNumSimulationSubSteps(4);
}

19
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -460,6 +460,15 @@ void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration)
}
void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps)
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetNumSubSteps(command, numSubSteps);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
}
b3RobotSimAPI::~b3RobotSimAPI()
{
delete m_data;
@@ -596,6 +605,16 @@ bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo*
return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0);
}
void b3RobotSimAPI::createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
{
b3SharedMemoryStatusHandle statusHandle;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClient));
if (b3CanSubmitCommand(m_data->m_physicsClient))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3CreateJoint(m_data->m_physicsClient, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
}
}
void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args)
{
b3SharedMemoryStatusHandle statusHandle;

4
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -87,12 +87,16 @@ public:
int getNumJoints(int bodyUniqueId) const;
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
void stepSimulation();
void setGravity(const b3Vector3& gravityAcceleration);
void setNumSimulationSubSteps(int numSubSteps);
void renderScene();
void debugDraw(int debugDrawMode);

33
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -154,6 +154,15 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double
return 0;
}
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS;
command->m_physSimParamArgs.m_numSimulationSubSteps = numSubSteps;
return 0;
}
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -730,6 +739,30 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
return cl->getJointInfo(bodyIndex, jointIndex, *info);
}
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_JOINT;
command->m_createJointArguments.m_parentBodyIndex = parentBodyIndex;
command->m_createJointArguments.m_parentJointIndex = parentJointIndex;
command->m_createJointArguments.m_childBodyIndex = childBodyIndex;
command->m_createJointArguments.m_childJointIndex = childJointIndex;
for (int i = 0; i < 7; ++i) {
command->m_createJointArguments.m_parentFrame[i] = info->m_parentFrame[i];
command->m_createJointArguments.m_childFrame[i] = info->m_childFrame[i];
}
for (int i = 0; i < 3; ++i) {
command->m_createJointArguments.m_jointAxis[i] = info->m_jointAxis[i];
}
command->m_createJointArguments.m_jointType = info->m_jointType;
return (b3SharedMemoryCommandHandle)command;
}
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -58,6 +58,8 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
@@ -82,6 +84,7 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);

74
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -11,6 +11,8 @@
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
@@ -384,6 +386,7 @@ struct PhysicsServerCommandProcessorInternalData
btScalar m_physicsDeltaTime;
btScalar m_numSimulationSubSteps;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
@@ -427,6 +430,7 @@ struct PhysicsServerCommandProcessorInternalData
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
m_numSimulationSubSteps(0),
m_dynamicsWorld(0),
m_remoteDebugDrawer(0),
m_guiHelper(0),
@@ -748,6 +752,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
u2b.activateModel(m);
btMultiBody* mb = 0;
btRigidBody* rb = 0;
//get a body index
int bodyUniqueId = m_data->allocHandle();
@@ -775,6 +780,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
mb = creation.getBulletMultiBody();
rb = creation.getRigidBody();
if (mb)
{
bodyHandle->m_multiBody = mb;
@@ -814,6 +820,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
} else
{
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
bodyHandle->m_rigidBody = rb;
}
}
@@ -878,6 +885,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
}
btMultiBody* mb = creation.getBulletMultiBody();
btRigidBody* rb = creation.getRigidBody();
if (useMultiBody)
{
@@ -946,8 +954,11 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
} else
{
return true;
if (rb)
{
bodyHandle->m_rigidBody = rb;
return true;
}
}
}
@@ -1804,7 +1815,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
applyJointDamping(i);
}
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0);
if (m_data->m_numSimulationSubSteps > 0)
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,m_data->m_numSimulationSubSteps,m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps);
else
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0);
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
@@ -1834,6 +1848,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS)
{
m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
@@ -2239,6 +2257,56 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_CREATE_JOINT:
{
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_createJointArguments.m_parentBodyIndex);
if (parentBody && parentBody->m_multiBody)
{
InteralBodyData* childBody = m_data->getHandle(clientCmd.m_createJointArguments.m_childBodyIndex);
if (childBody)
{
btVector3 pivotInParent(clientCmd.m_createJointArguments.m_parentFrame[0], clientCmd.m_createJointArguments.m_parentFrame[1], clientCmd.m_createJointArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_createJointArguments.m_childFrame[0], clientCmd.m_createJointArguments.m_childFrame[1], clientCmd.m_createJointArguments.m_childFrame[2]);
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6]));
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6]));
btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]);
if (clientCmd.m_createJointArguments.m_jointType == btMultibodyLink::eFixed)
{
if (childBody->m_multiBody)
{
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
multibodyFixed->setMaxAppliedImpulse(2.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
}
else
{
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(2.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed);
}
}
else if (clientCmd.m_createJointArguments.m_jointType == btMultibodyLink::ePrismatic)
{
if (childBody->m_multiBody)
{
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
multibodySlider->setMaxAppliedImpulse(2.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
}
else
{
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
rigidbodySlider->setMaxAppliedImpulse(2.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider);
}
}
}
}
hasStatus = true;
break;
}
default:
{
b3Error("Unknown command encountered");

13
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -366,6 +366,18 @@ struct CalculateInverseDynamicsResultArgs
double m_jointForces[MAX_DEGREE_OF_FREEDOM];
};
struct CreateJointArgs
{
int m_parentBodyIndex;
int m_parentJointIndex;
int m_childBodyIndex;
int m_childJointIndex;
double m_parentFrame[7];
double m_childFrame[7];
double m_jointAxis[3];
int m_jointType;
};
struct SharedMemoryCommand
{
int m_type;
@@ -393,6 +405,7 @@ struct SharedMemoryCommand
struct PickBodyArgs m_pickBodyArguments;
struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CreateJointArgs m_createJointArguments;
};
};

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -28,7 +28,8 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_CAMERA_IMAGE_DATA,
CMD_APPLY_EXTERNAL_FORCE,
CMD_CALCULATE_INVERSE_DYNAMICS,
CMD_MAX_CLIENT_COMMANDS
CMD_MAX_CLIENT_COMMANDS,
CMD_CREATE_JOINT
};
enum EnumSharedMemoryServerStatus
@@ -99,6 +100,9 @@ struct b3JointInfo
int m_flags;
double m_jointDamping;
double m_jointFriction;
double m_parentFrame[7]; // position and orientation (quaternion)
double m_childFrame[7]; // ^^^
double m_jointAxis[3]; // joint axis in parent local frame
};
struct b3JointSensorState

2
src/BulletDynamics/CMakeLists.txt
View File

@@ -33,6 +33,7 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyConstraint.cpp
Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodyFixedConstraint.cpp
Featherstone/btMultiBodySliderConstraint.cpp
Featherstone/btMultiBodyJointMotor.cpp
MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp
@@ -91,6 +92,7 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyPoint2Point.h
Featherstone/btMultiBodyFixedConstraint.h
Featherstone/btMultiBodySliderConstraint.h
Featherstone/btMultiBodyJointMotor.h
)

13
src/BulletDynamics/Character/btKinematicCharacterController.cpp
View File

@@ -453,16 +453,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
break;
}
} else {
// if (!m_wasJumping)
// we moved whole way
m_currentPosition = m_targetPosition;
}
m_currentPosition = m_targetPosition;
// if (callback.m_closestHitFraction == 0.f)
// break;
else
{
m_currentPosition = m_targetPosition;
}
}
}

230
src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp Normal file
View File

@@ -0,0 +1,230 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#include "btMultiBodySliderConstraint.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "LinearMath/btIDebugDraw.h"
#define BTMBSLIDERCONSTRAINT_DIM 5
#define EPSILON 0.000001
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
:btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB),
m_jointAxis(jointAxis)
{
m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
}
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBSLIDERCONSTRAINT_DIM,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_frameInA(frameInA),
m_frameInB(frameInB),
m_jointAxis(jointAxis)
{
m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
}
void btMultiBodySliderConstraint::finalizeMultiDof()
{
//not implemented yet
btAssert(0);
}
btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
{
}
int btMultiBodySliderConstraint::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodySliderConstraint::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
btMatrix3x3 frameAworld = m_frameInA;
btVector3 jointAxis = m_jointAxis;
if (m_rigidBodyA)
{
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
frameAworld = m_frameInA.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation());
jointAxis = quatRotate(m_rigidBodyA->getOrientation(),m_jointAxis);
} else if (m_bodyA) {
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
}
btVector3 pivotBworld = m_pivotInB;
btMatrix3x3 frameBworld = m_frameInB;
if (m_rigidBodyB)
{
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
frameBworld = m_frameInB.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation());
} else if (m_bodyB) {
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
}
btVector3 constraintAxis[2];
for (int i = 0; i < 3; ++i)
{
constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
if (constraintAxis[0].norm() > EPSILON)
{
constraintAxis[0] = constraintAxis[0].normalized();
constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
constraintAxis[1] = constraintAxis[1].normalized();
break;
}
}
btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
btVector3 angleDiff;
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff);
int numDim = BTMBSLIDERCONSTRAINT_DIM;
for (int i=0;i<numDim;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = i;
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal1.setValue(0,0,0);
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
constraintRow.m_contactNormal2.setValue(0,0,0);
constraintRow.m_angularComponentA.setValue(0,0,0);
constraintRow.m_angularComponentB.setValue(0,0,0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
if (m_rigidBodyA)
{
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
}
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
}
btVector3 constraintNormalLin(0,0,0);
btVector3 constraintNormalAng(0,0,0);
btScalar posError = 0.0;
if (i < 2) {
constraintNormalLin = constraintAxis[i];
posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse
);
}
else { //i>=2
constraintNormalAng = frameAworld.getColumn(i%3);
posError = angleDiff[i%3];
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse, true
);
}
}
}
void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
{
btTransform tr;
tr.setIdentity();
if (m_rigidBodyA)
{
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyA)
{
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
tr.setOrigin(pivotAworld);
drawer->drawTransform(tr, 0.1);
}
if (m_rigidBodyB)
{
// that ideally should draw the same frame
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
tr.setOrigin(pivot);
drawer->drawTransform(tr, 0.1);
}
if (m_bodyB)
{
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
tr.setOrigin(pivotBworld);
drawer->drawTransform(tr, 0.1);
}
}

105
src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h Normal file
View File

@@ -0,0 +1,105 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
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.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H
#define BT_MULTIBODY_SLIDER_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodySliderConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btVector3 m_jointAxis;
public:
btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
virtual ~btMultiBodySliderConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
const btVector3& getJointAxis() const
{
return m_jointAxis;
}
void setJointAxis(const btVector3& jointAxis)
{
m_jointAxis = jointAxis;
}
virtual void debugDraw(class btIDebugDraw* drawer);
};
#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H