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

Merge pull request #1990 from erwincoumans/master

Browse Source 
If a convex has called 'initializePolyhedralFeatures', use those feat…
This commit is contained in:
erwincoumans
2018-11-13 15:32:05 -08:00
committed by GitHub
parent 75d60e1550 7c7235a8c5
commit 9ad77a220e
15 changed files with 842 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/BulletDynamics/CMakeLists.txt
View File

@@ -41,6 +41,7 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyMLCPConstraintSolver.cpp
Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodySliderConstraint.cpp
Featherstone/btMultiBodySphericalJointMotor.cpp
MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp
MLCPSolvers/btLemkeAlgorithm.cpp

54
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -37,19 +37,21 @@ const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
} // namespace
namespace
{
void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
const btVector3 &top_in, // top part of input vector
const btVector3 &bottom_in, // bottom part of input vector
btVector3 &top_out, // top part of output vector
btVector3 &bottom_out) // bottom part of output vector
void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
const btVector3 &top_in, // top part of input vector
const btVector3 &bottom_in, // bottom part of input vector
btVector3 &top_out, // top part of output vector
btVector3 &bottom_out) // bottom part of output vector
{
top_out = rotation_matrix * top_in;
bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
}
namespace
{
#if 0
void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
const btVector3 &displacement,
@@ -330,6 +332,9 @@ void btMultiBody::setupPlanar(int i,
m_links[i].updateCacheMultiDof();
//
updateLinksDofOffsets();
m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
}
void btMultiBody::finalizeMultiDof()
@@ -540,35 +545,26 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
{
int num_links = getNumLinks();
// Calculates the velocities of each link (and the base) in its local frame
omega[0] = quatRotate(m_baseQuat, getBaseOmega());
vel[0] = quatRotate(m_baseQuat, getBaseVel());
const btQuaternion& base_rot = getWorldToBaseRot();
omega[0] = quatRotate(base_rot, getBaseOmega());
vel[0] = quatRotate(base_rot, getBaseVel());
for (int i = 0; i < num_links; ++i)
{
const int parent = m_links[i].m_parent;
const btMultibodyLink& link = getLink(i);
const int parent = link.m_parent;
// transform parent vel into this frame, store in omega[i+1], vel[i+1]
SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
omega[parent + 1], vel[parent + 1],
omega[i + 1], vel[i + 1]);
spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,
omega[parent + 1], vel[parent + 1],
omega[i + 1], vel[i + 1]);
// now add qidot * shat_i
//only supported for revolute/prismatic joints, todo: spherical and planar joints
switch (m_links[i].m_jointType)
const btScalar* jointVel = getJointVelMultiDof(i);
for (int dof = 0; dof < link.m_dofCount; ++dof)
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
btVector3 axisTop = m_links[i].getAxisTop(0);
btVector3 axisBottom = m_links[i].getAxisBottom(0);
btScalar jointVel = getJointVel(i);
omega[i + 1] += jointVel * axisTop;
vel[i + 1] += jointVel * axisBottom;
break;
}
default:
{
}
omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
}
}
}

9
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -600,6 +600,15 @@ public:
m_userIndex2 = index;
}
static void spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
const btVector3 &top_in, // top part of input vector
const btVector3 &bottom_in, // bottom part of input vector
btVector3 &top_out, // top part of output vector
btVector3 &bottom_out); // bottom part of output vector
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented

172
src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp Normal file
View File

@@ -0,0 +1,172 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2018 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 "btMultiBodySphericalJointMotor.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "LinearMath/btTransformUtil.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true),
m_desiredVelocity(0, 0, 0),
m_desiredPosition(0,0,0,1),
m_kd(1.),
m_kp(0.2),
m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{
m_maxAppliedImpulse = maxMotorImpulse;
}
void btMultiBodySphericalJointMotor::finalizeMultiDof()
{
allocateJacobiansMultiDof();
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
// row 0: the lower bound
// row 0: the lower bound
jacobianA(0)[offset] = 1;
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
{
}
int btMultiBodySphericalJointMotor::getIslandIdA() const
{
if (this->m_linkA < 0)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
{
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodySphericalJointMotor::getIslandIdB() const
{
if (m_linkB < 0)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
{
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}
void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse == 0.f)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
btQuaternion desiredQuat = m_desiredPosition;
btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
m_bodyA->getJointPosMultiDof(m_linkA)[1],
m_bodyA->getJointPosMultiDof(m_linkA)[2],
m_bodyA->getJointPosMultiDof(m_linkA)[3]);
btQuaternion relRot = currentQuat.inverse() * desiredQuat;
btVector3 angleDiff;
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
for (int row = 0; row < getNumRows(); row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = row;
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar desiredVelocity = this->m_desiredVelocity[row];
btScalar velocityError = desiredVelocity - currentVelocity;
btMatrix3x3 frameAworld;
frameAworld.setIdentity();
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
btScalar posError = 0;
{
btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eSpherical:
{
btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
posError = m_kp*angleDiff[row % 3];
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
btVector3(0,0,0), dummy, dummy,
posError,
infoGlobal,
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
break;
}
default:
{
btAssert(0);
}
};
}
}
}

77
src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h Normal file
View File

@@ -0,0 +1,77 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2018 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_SPHERICAL_JOINT_MOTOR_H
#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodySphericalJointMotor : public btMultiBodyConstraint
{
protected:
btVector3 m_desiredVelocity;
btQuaternion m_desiredPosition;
btScalar m_kd;
btScalar m_kp;
btScalar m_erp;
btScalar m_rhsClamp; //maximum error
public:
btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse);
virtual ~btMultiBodySphericalJointMotor();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f)
{
m_desiredVelocity = velTarget;
m_kd = kd;
}
virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f)
{
m_desiredPosition = posTarget;
m_kp = kp;
}
virtual void setErp(btScalar erp)
{
m_erp = erp;
}
virtual btScalar getErp() const
{
return m_erp;
}
virtual void setRhsClamp(btScalar rhsClamp)
{
m_rhsClamp = rhsClamp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
};
#endif //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H