Added a first version of a Featherstone multi body implementation.
The original version was written by Stephen Thompson. I replaced Eigen math by Bullet LinearMath, and added a dedicated 6x6 matrix solver. Also I integrated support for collisions/contact constraints between btMultiBody and btRigidBody, and de-activation support. See Demos/FeatherstoneMultiBodyDemo/Win32FeatherstoneMultiBodyDemo.cpp for example usage. There is currently only support for contact constraints for btMultiBody. Next on the list will be adding support for joint limit constraint for btMultiBody. The implementation is still experimental/untested, the quality will improve in upcoming Bullet releases.
This commit is contained in:
erwin.coumans
988
src/BulletDynamics/Featherstone/btMultiBody.cpp
Normal file
988
src/BulletDynamics/Featherstone/btMultiBody.cpp
Normal file
@@ -0,0 +1,988 @@
|
||||
/*
|
||||
* PURPOSE:
|
||||
* Class representing an articulated rigid body. Stores the body's
|
||||
* current state, allows forces and torques to be set, handles
|
||||
* timestepping and implements Featherstone's algorithm.
|
||||
*
|
||||
* COPYRIGHT:
|
||||
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
|
||||
* Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#include "btMultiBody.h"
|
||||
#include "btMultiBodyLink.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
|
||||
// #define INCLUDE_GYRO_TERM
|
||||
|
||||
namespace {
|
||||
const btScalar SLEEP_EPSILON = btScalar(0.01); // this is a squared velocity (m^2 s^-2)
|
||||
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
// Various spatial helper functions
|
||||
//
|
||||
|
||||
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
|
||||
{
|
||||
top_out = rotation_matrix * top_in;
|
||||
bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
|
||||
}
|
||||
|
||||
void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
|
||||
const btVector3 &displacement,
|
||||
const btVector3 &top_in,
|
||||
const btVector3 &bottom_in,
|
||||
btVector3 &top_out,
|
||||
btVector3 &bottom_out)
|
||||
{
|
||||
top_out = rotation_matrix.transpose() * top_in;
|
||||
bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
|
||||
}
|
||||
|
||||
btScalar SpatialDotProduct(const btVector3 &a_top,
|
||||
const btVector3 &a_bottom,
|
||||
const btVector3 &b_top,
|
||||
const btVector3 &b_bottom)
|
||||
{
|
||||
return a_bottom.dot(b_top) + a_top.dot(b_bottom);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// Implementation of class btMultiBody
|
||||
//
|
||||
|
||||
btMultiBody::btMultiBody(int n_links,
|
||||
btScalar mass,
|
||||
const btVector3 &inertia,
|
||||
bool fixed_base_,
|
||||
bool can_sleep_)
|
||||
: num_links(n_links),
|
||||
base_quat(0, 0, 0, 1),
|
||||
base_mass(mass),
|
||||
base_inertia(inertia),
|
||||
|
||||
fixed_base(fixed_base_),
|
||||
awake(true),
|
||||
can_sleep(can_sleep_),
|
||||
sleep_timer(0)
|
||||
{
|
||||
links.resize(n_links);
|
||||
|
||||
vector_buf.resize(2*n_links);
|
||||
matrix_buf.resize(n_links + 1);
|
||||
real_buf.resize(6 + 2*n_links);
|
||||
base_pos.setValue(0, 0, 0);
|
||||
base_force.setValue(0, 0, 0);
|
||||
base_torque.setValue(0, 0, 0);
|
||||
}
|
||||
|
||||
btMultiBody::~btMultiBody()
|
||||
{
|
||||
}
|
||||
|
||||
void btMultiBody::setupPrismatic(int i,
|
||||
btScalar mass,
|
||||
const btVector3 &inertia,
|
||||
int parent,
|
||||
const btQuaternion &rot_parent_to_this,
|
||||
const btVector3 &joint_axis,
|
||||
const btVector3 &r_vector_when_q_zero)
|
||||
{
|
||||
links[i].mass = mass;
|
||||
links[i].inertia = inertia;
|
||||
links[i].parent = parent;
|
||||
links[i].zero_rot_parent_to_this = rot_parent_to_this;
|
||||
links[i].axis_top.setValue(0,0,0);
|
||||
links[i].axis_bottom = joint_axis;
|
||||
links[i].e_vector = r_vector_when_q_zero;
|
||||
links[i].is_revolute = false;
|
||||
links[i].cached_rot_parent_to_this = rot_parent_to_this;
|
||||
links[i].updateCache();
|
||||
}
|
||||
|
||||
void btMultiBody::setupRevolute(int i,
|
||||
btScalar mass,
|
||||
const btVector3 &inertia,
|
||||
int parent,
|
||||
const btQuaternion &zero_rot_parent_to_this,
|
||||
const btVector3 &joint_axis,
|
||||
const btVector3 &parent_axis_position,
|
||||
const btVector3 &my_axis_position)
|
||||
{
|
||||
links[i].mass = mass;
|
||||
links[i].inertia = inertia;
|
||||
links[i].parent = parent;
|
||||
links[i].zero_rot_parent_to_this = zero_rot_parent_to_this;
|
||||
links[i].axis_top = joint_axis;
|
||||
links[i].axis_bottom = joint_axis.cross(my_axis_position);
|
||||
links[i].d_vector = my_axis_position;
|
||||
links[i].e_vector = parent_axis_position;
|
||||
links[i].is_revolute = true;
|
||||
links[i].updateCache();
|
||||
}
|
||||
|
||||
void btMultiBody::addLinkCollider(btMultiBodyLinkCollider* collider)
|
||||
{
|
||||
m_colliders.push_back(collider);
|
||||
}
|
||||
|
||||
btMultiBodyLinkCollider* btMultiBody::getLinkCollider(int index)
|
||||
{
|
||||
return m_colliders[index];
|
||||
}
|
||||
const btMultiBodyLinkCollider* btMultiBody::getLinkCollider(int index) const
|
||||
{
|
||||
return m_colliders[index];
|
||||
}
|
||||
|
||||
int btMultiBody::getNumLinkColliders() const
|
||||
{
|
||||
return m_colliders.size();
|
||||
}
|
||||
|
||||
|
||||
int btMultiBody::getParent(int i) const
|
||||
{
|
||||
return links[i].parent;
|
||||
}
|
||||
|
||||
btScalar btMultiBody::getLinkMass(int i) const
|
||||
{
|
||||
return links[i].mass;
|
||||
}
|
||||
|
||||
const btVector3 & btMultiBody::getLinkInertia(int i) const
|
||||
{
|
||||
return links[i].inertia;
|
||||
}
|
||||
|
||||
btScalar btMultiBody::getJointPos(int i) const
|
||||
{
|
||||
return links[i].joint_pos;
|
||||
}
|
||||
|
||||
btScalar btMultiBody::getJointVel(int i) const
|
||||
{
|
||||
return real_buf[6 + i];
|
||||
}
|
||||
|
||||
void btMultiBody::setJointPos(int i, btScalar q)
|
||||
{
|
||||
links[i].joint_pos = q;
|
||||
links[i].updateCache();
|
||||
}
|
||||
|
||||
void btMultiBody::setJointVel(int i, btScalar qdot)
|
||||
{
|
||||
real_buf[6 + i] = qdot;
|
||||
}
|
||||
|
||||
const btVector3 & btMultiBody::getRVector(int i) const
|
||||
{
|
||||
return links[i].cached_r_vector;
|
||||
}
|
||||
|
||||
const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
|
||||
{
|
||||
return links[i].cached_rot_parent_to_this;
|
||||
}
|
||||
|
||||
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
|
||||
{
|
||||
btVector3 result = local_pos;
|
||||
while (i != -1) {
|
||||
// 'result' is in frame i. transform it to frame parent(i)
|
||||
result += getRVector(i);
|
||||
result = quatRotate(getParentToLocalRot(i).inverse(),result);
|
||||
i = getParent(i);
|
||||
}
|
||||
|
||||
// 'result' is now in the base frame. transform it to world frame
|
||||
result = quatRotate(getWorldToBaseRot().inverse() ,result);
|
||||
result += getBasePos();
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
|
||||
{
|
||||
if (i == -1) {
|
||||
// world to base
|
||||
return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
|
||||
} else {
|
||||
// find position in parent frame, then transform to current frame
|
||||
return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
|
||||
}
|
||||
}
|
||||
|
||||
btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
|
||||
{
|
||||
btVector3 result = local_dir;
|
||||
while (i != -1) {
|
||||
result = quatRotate(getParentToLocalRot(i).inverse() , result);
|
||||
i = getParent(i);
|
||||
}
|
||||
result = quatRotate(getWorldToBaseRot().inverse() , result);
|
||||
return result;
|
||||
}
|
||||
|
||||
btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
|
||||
{
|
||||
if (i == -1) {
|
||||
return quatRotate(getWorldToBaseRot(), world_dir);
|
||||
} else {
|
||||
return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
|
||||
{
|
||||
// Calculates the velocities of each link (and the base) in its local frame
|
||||
omega[0] = quatRotate(base_quat ,getBaseOmega());
|
||||
vel[0] = quatRotate(base_quat ,getBaseVel());
|
||||
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
const int parent = links[i].parent;
|
||||
|
||||
// transform parent vel into this frame, store in omega[i+1], vel[i+1]
|
||||
SpatialTransform(btMatrix3x3(links[i].cached_rot_parent_to_this), links[i].cached_r_vector,
|
||||
omega[parent+1], vel[parent+1],
|
||||
omega[i+1], vel[i+1]);
|
||||
|
||||
// now add qidot * shat_i
|
||||
omega[i+1] += getJointVel(i) * links[i].axis_top;
|
||||
vel[i+1] += getJointVel(i) * links[i].axis_bottom;
|
||||
}
|
||||
}
|
||||
|
||||
btScalar btMultiBody::getKineticEnergy() const
|
||||
{
|
||||
// TODO: would be better not to allocate memory here
|
||||
btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
|
||||
btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
|
||||
compTreeLinkVelocities(&omega[0], &vel[0]);
|
||||
|
||||
// we will do the factor of 0.5 at the end
|
||||
btScalar result = base_mass * vel[0].dot(vel[0]);
|
||||
result += omega[0].dot(base_inertia * omega[0]);
|
||||
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
result += links[i].mass * vel[i+1].dot(vel[i+1]);
|
||||
result += omega[i+1].dot(links[i].inertia * omega[i+1]);
|
||||
}
|
||||
|
||||
return 0.5f * result;
|
||||
}
|
||||
|
||||
btVector3 btMultiBody::getAngularMomentum() const
|
||||
{
|
||||
// TODO: would be better not to allocate memory here
|
||||
btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
|
||||
btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
|
||||
btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
|
||||
compTreeLinkVelocities(&omega[0], &vel[0]);
|
||||
|
||||
rot_from_world[0] = base_quat;
|
||||
btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0]));
|
||||
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1];
|
||||
result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1])));
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
void btMultiBody::clearForcesAndTorques()
|
||||
{
|
||||
base_force.setValue(0, 0, 0);
|
||||
base_torque.setValue(0, 0, 0);
|
||||
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
links[i].applied_force.setValue(0, 0, 0);
|
||||
links[i].applied_torque.setValue(0, 0, 0);
|
||||
links[i].joint_torque = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBody::clearVelocities()
|
||||
{
|
||||
for (int i = 0; i < 6 + num_links; ++i)
|
||||
{
|
||||
real_buf[i] = 0.f;
|
||||
}
|
||||
}
|
||||
void btMultiBody::addLinkForce(int i, const btVector3 &f)
|
||||
{
|
||||
links[i].applied_force += f;
|
||||
}
|
||||
|
||||
void btMultiBody::addLinkTorque(int i, const btVector3 &t)
|
||||
{
|
||||
links[i].applied_torque += t;
|
||||
}
|
||||
|
||||
void btMultiBody::addJointTorque(int i, btScalar Q)
|
||||
{
|
||||
links[i].joint_torque += Q;
|
||||
}
|
||||
|
||||
const btVector3 & btMultiBody::getLinkForce(int i) const
|
||||
{
|
||||
return links[i].applied_force;
|
||||
}
|
||||
|
||||
const btVector3 & btMultiBody::getLinkTorque(int i) const
|
||||
{
|
||||
return links[i].applied_torque;
|
||||
}
|
||||
|
||||
btScalar btMultiBody::getJointTorque(int i) const
|
||||
{
|
||||
return links[i].joint_torque;
|
||||
}
|
||||
|
||||
|
||||
inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Transposed)
|
||||
{
|
||||
btVector3 row0 = btVector3(
|
||||
v0.x() * v1Transposed.x(),
|
||||
v0.x() * v1Transposed.y(),
|
||||
v0.x() * v1Transposed.z());
|
||||
btVector3 row1 = btVector3(
|
||||
v0.y() * v1Transposed.x(),
|
||||
v0.y() * v1Transposed.y(),
|
||||
v0.y() * v1Transposed.z());
|
||||
btVector3 row2 = btVector3(
|
||||
v0.z() * v1Transposed.x(),
|
||||
v0.z() * v1Transposed.y(),
|
||||
v0.z() * v1Transposed.z());
|
||||
|
||||
btMatrix3x3 m(row0[0],row0[1],row0[2],
|
||||
row1[0],row1[1],row1[2],
|
||||
row2[0],row2[1],row2[2]);
|
||||
return m;
|
||||
}
|
||||
|
||||
|
||||
void btMultiBody::stepVelocities(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.
|
||||
|
||||
const btScalar DAMPING_K1 = btScalar(0.0);
|
||||
//const btScalar DAMPING_K2 = btScalar(0);
|
||||
const btScalar DAMPING_K2 = btScalar(0.0);
|
||||
|
||||
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*num_links + 6);
|
||||
scratch_v.resize(8*num_links + 6);
|
||||
scratch_m.resize(4*num_links + 4);
|
||||
|
||||
btScalar * r_ptr = &scratch_r[0];
|
||||
btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
|
||||
btVector3 * v_ptr = &scratch_v[0];
|
||||
|
||||
// vhat_i (top = angular, bottom = linear part)
|
||||
btVector3 * vel_top = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * vel_bottom = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// zhat_i^A
|
||||
btVector3 * zero_acc_top = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * zero_acc_bottom = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// chat_i (note NOT defined for the base)
|
||||
btVector3 * coriolis_top = v_ptr; v_ptr += num_links;
|
||||
btVector3 * coriolis_bottom = 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 = &matrix_buf[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 = num_links > 0 ? &vector_buf[0] : 0;
|
||||
btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
|
||||
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// Y_i, D_i
|
||||
btScalar * Y = r_ptr; r_ptr += num_links;
|
||||
btScalar * D = num_links > 0 ? &real_buf[6 + num_links] : 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(base_quat);
|
||||
|
||||
vel_top[0] = rot_from_parent[0] * base_omega;
|
||||
vel_bottom[0] = rot_from_parent[0] * base_vel;
|
||||
|
||||
if (fixed_base) {
|
||||
zero_acc_top[0] = zero_acc_bottom[0] = btVector3(0,0,0);
|
||||
} else {
|
||||
zero_acc_top[0] = - (rot_from_parent[0] * (base_force
|
||||
- base_mass*(DAMPING_K1+DAMPING_K2*base_vel.norm())*base_vel));
|
||||
zero_acc_bottom[0] =
|
||||
#ifdef INCLUDE_GYRO_TERM
|
||||
vel_top[0].cross( base_inertia.cwise() * vel_top[0] )
|
||||
#endif
|
||||
- (rot_from_parent[0] * base_torque);
|
||||
zero_acc_bottom[0] += base_inertia * vel_top[0] * (DAMPING_K1 + DAMPING_K2*vel_top[0].norm());
|
||||
}
|
||||
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
|
||||
|
||||
|
||||
inertia_top_right[0].setValue(base_mass, 0, 0,
|
||||
0, base_mass, 0,
|
||||
0, 0, base_mass);
|
||||
inertia_bottom_left[0].setValue(base_inertia[0], 0, 0,
|
||||
0, base_inertia[1], 0,
|
||||
0, 0, base_inertia[2]);
|
||||
|
||||
rot_from_world[0] = rot_from_parent[0];
|
||||
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
const int parent = links[i].parent;
|
||||
rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this);
|
||||
|
||||
|
||||
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], links[i].cached_r_vector,
|
||||
vel_top[parent+1], vel_bottom[parent+1],
|
||||
vel_top[i+1], vel_bottom[i+1]);
|
||||
|
||||
// we can now calculate chat_i
|
||||
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
|
||||
coriolis_bottom[i] = vel_top[i+1].cross(vel_top[i+1].cross(links[i].cached_r_vector))
|
||||
+ 2 * vel_top[i+1].cross(links[i].axis_bottom) * getJointVel(i);
|
||||
if (links[i].is_revolute) {
|
||||
coriolis_top[i] = vel_top[i+1].cross(links[i].axis_top) * getJointVel(i);
|
||||
coriolis_bottom[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom);
|
||||
} else {
|
||||
coriolis_top[i] = btVector3(0,0,0);
|
||||
}
|
||||
|
||||
// now set vhat_i to its true value by doing
|
||||
// vhat_i += qidot * shat_i
|
||||
vel_top[i+1] += getJointVel(i) * links[i].axis_top;
|
||||
vel_bottom[i+1] += getJointVel(i) * links[i].axis_bottom;
|
||||
|
||||
// calculate zhat_i^A
|
||||
zero_acc_top[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
|
||||
zero_acc_top[i+1] += links[i].mass * (DAMPING_K1 + DAMPING_K2*vel_bottom[i+1].norm()) * vel_bottom[i+1];
|
||||
|
||||
zero_acc_bottom[i+1] =
|
||||
#ifdef INCLUDE_GYRO_TERM
|
||||
vel_top[i+1].cross( links[i].inertia.cwise() * vel_top[i+1] )
|
||||
#endif
|
||||
- (rot_from_world[i+1] * links[i].applied_torque);
|
||||
zero_acc_bottom[i+1] += links[i].inertia * vel_top[i+1] * (DAMPING_K1 + DAMPING_K2*vel_top[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(links[i].mass, 0, 0,
|
||||
0, links[i].mass, 0,
|
||||
0, 0, links[i].mass);
|
||||
inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0,
|
||||
0, links[i].inertia[1], 0,
|
||||
0, 0, links[i].inertia[2]);
|
||||
}
|
||||
|
||||
|
||||
// 'Downward' loop.
|
||||
// (part of TreeForwardDynamics in Mirtich.)
|
||||
for (int i = num_links - 1; i >= 0; --i) {
|
||||
|
||||
h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom;
|
||||
h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom;
|
||||
|
||||
D[i] = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
|
||||
Y[i] = links[i].joint_torque
|
||||
- SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top[i+1], zero_acc_bottom[i+1])
|
||||
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top[i], coriolis_bottom[i]);
|
||||
|
||||
const int parent = links[i].parent;
|
||||
|
||||
|
||||
// Ip += pXi * (Ii - hi hi' / Di) * iXp
|
||||
const btScalar one_over_di = 1.0f / D[i];
|
||||
|
||||
|
||||
|
||||
|
||||
const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
|
||||
const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
|
||||
const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
|
||||
|
||||
|
||||
btMatrix3x3 r_cross;
|
||||
r_cross.setValue(
|
||||
0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1],
|
||||
links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0],
|
||||
-links[i].cached_r_vector[1], links[i].cached_r_vector[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];
|
||||
|
||||
|
||||
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
|
||||
btVector3 in_top, in_bottom, out_top, out_bottom;
|
||||
const btScalar Y_over_D = Y[i] * one_over_di;
|
||||
in_top = zero_acc_top[i+1]
|
||||
+ inertia_top_left[i+1] * coriolis_top[i]
|
||||
+ inertia_top_right[i+1] * coriolis_bottom[i]
|
||||
+ Y_over_D * h_top[i];
|
||||
in_bottom = zero_acc_bottom[i+1]
|
||||
+ inertia_bottom_left[i+1] * coriolis_top[i]
|
||||
+ inertia_top_left[i+1].transpose() * coriolis_bottom[i]
|
||||
+ Y_over_D * h_bottom[i];
|
||||
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
|
||||
in_top, in_bottom, out_top, out_bottom);
|
||||
zero_acc_top[parent+1] += out_top;
|
||||
zero_acc_bottom[parent+1] += out_bottom;
|
||||
}
|
||||
|
||||
|
||||
// Second 'upward' loop
|
||||
// (part of TreeForwardDynamics in Mirtich)
|
||||
|
||||
if (fixed_base)
|
||||
{
|
||||
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (num_links > 0)
|
||||
{
|
||||
//Matrix<btScalar, 6, 6> Imatrix;
|
||||
//Imatrix.block<3,3>(0,0) = inertia_top_left[0];
|
||||
//Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
|
||||
//Imatrix.block<3,3>(0,3) = inertia_top_right[0];
|
||||
//Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
|
||||
//cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
|
||||
|
||||
cached_inertia_top_left = inertia_top_left[0];
|
||||
cached_inertia_top_right = inertia_top_right[0];
|
||||
cached_inertia_lower_left = inertia_bottom_left[0];
|
||||
cached_inertia_lower_right= inertia_top_left[0].transpose();
|
||||
|
||||
}
|
||||
btVector3 rhs_top (zero_acc_top[0][0], zero_acc_top[0][1], zero_acc_top[0][2]);
|
||||
btVector3 rhs_bot (zero_acc_bottom[0][0], zero_acc_bottom[0][1], zero_acc_bottom[0][2]);
|
||||
float result[6];
|
||||
|
||||
solveImatrix(rhs_top, rhs_bot, result);
|
||||
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
accel_top[0][i] = -result[i];
|
||||
accel_bottom[0][i] = -result[i+3];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// now do the loop over the links
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
const int parent = links[i].parent;
|
||||
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
|
||||
accel_top[parent+1], accel_bottom[parent+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];
|
||||
accel_top[i+1] += coriolis_top[i] + joint_accel[i] * links[i].axis_top;
|
||||
accel_bottom[i+1] += coriolis_bottom[i] + joint_accel[i] * links[i].axis_bottom;
|
||||
}
|
||||
|
||||
// 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];
|
||||
// Final step: add the accelerations (times dt) to the velocities.
|
||||
applyDeltaVee(output, dt);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
|
||||
{
|
||||
///solve I * x = rhs, so the result = invI * rhs
|
||||
if (num_links == 0)
|
||||
{
|
||||
// in the case of 0 links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
|
||||
result[0] = rhs_bot[0] / base_inertia[0];
|
||||
result[1] = rhs_bot[1] / base_inertia[1];
|
||||
result[2] = rhs_bot[2] / base_inertia[2];
|
||||
result[3] = rhs_top[0] / base_mass;
|
||||
result[4] = rhs_top[1] / base_mass;
|
||||
result[5] = rhs_top[2] / base_mass;
|
||||
} else
|
||||
{
|
||||
/// Special routine for calculating the inverse of a spatial inertia matrix
|
||||
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
|
||||
btMatrix3x3 Binv = cached_inertia_top_right.inverse()*-1.f;
|
||||
btMatrix3x3 tmp = cached_inertia_lower_right * Binv;
|
||||
btMatrix3x3 invIupper_right = (tmp * cached_inertia_top_left + cached_inertia_lower_left).inverse();
|
||||
tmp = invIupper_right * cached_inertia_lower_right;
|
||||
btMatrix3x3 invI_upper_left = (tmp * Binv);
|
||||
btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
|
||||
tmp = cached_inertia_top_left * invI_upper_left;
|
||||
tmp[0][0]-= 1.0;
|
||||
tmp[1][1]-= 1.0;
|
||||
tmp[2][2]-= 1.0;
|
||||
btMatrix3x3 invI_lower_left = (Binv * tmp);
|
||||
|
||||
//multiply result = invI * rhs
|
||||
{
|
||||
btVector3 vtop = invI_upper_left*rhs_top;
|
||||
btVector3 tmp;
|
||||
tmp = invIupper_right * rhs_bot;
|
||||
vtop += tmp;
|
||||
btVector3 vbot = invI_lower_left*rhs_top;
|
||||
tmp = invI_lower_right * rhs_bot;
|
||||
vbot += tmp;
|
||||
result[0] = vtop[0];
|
||||
result[1] = vtop[1];
|
||||
result[2] = vtop[2];
|
||||
result[3] = vbot[0];
|
||||
result[4] = vbot[1];
|
||||
result[5] = vbot[2];
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btMultiBody::calcAccelerationDeltas(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
|
||||
|
||||
scratch_r.resize(num_links);
|
||||
scratch_v.resize(4*num_links + 4);
|
||||
|
||||
btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
|
||||
btVector3 * v_ptr = &scratch_v[0];
|
||||
|
||||
// zhat_i^A (scratch space)
|
||||
btVector3 * zero_acc_top = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * zero_acc_bottom = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// rot_from_parent (cached from calcAccelerations)
|
||||
const btMatrix3x3 * rot_from_parent = &matrix_buf[0];
|
||||
|
||||
// hhat (cached), accel (scratch)
|
||||
const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
|
||||
const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
|
||||
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// Y_i (scratch), D_i (cached)
|
||||
btScalar * Y = r_ptr; r_ptr += num_links;
|
||||
const btScalar * D = num_links > 0 ? &real_buf[6 + num_links] : 0;
|
||||
|
||||
btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
|
||||
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
|
||||
|
||||
|
||||
|
||||
// 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 (fixed_base)
|
||||
{
|
||||
zero_acc_top[0] = zero_acc_bottom[0] = btVector3(0,0,0);
|
||||
} else
|
||||
{
|
||||
zero_acc_top[0] = - (rot_from_parent[0] * input_force);
|
||||
zero_acc_bottom[0] = - (rot_from_parent[0] * input_torque);
|
||||
}
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
zero_acc_top[i+1] = zero_acc_bottom[i+1] = btVector3(0,0,0);
|
||||
}
|
||||
|
||||
// 'Downward' loop.
|
||||
for (int i = num_links - 1; i >= 0; --i)
|
||||
{
|
||||
|
||||
Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top[i+1], zero_acc_bottom[i+1]);
|
||||
Y[i] += force[6 + i]; // add joint torque
|
||||
|
||||
const int parent = links[i].parent;
|
||||
|
||||
// Zp += pXi * (Zi + hi*Yi/Di)
|
||||
btVector3 in_top, in_bottom, out_top, out_bottom;
|
||||
const btScalar Y_over_D = Y[i] / D[i];
|
||||
in_top = zero_acc_top[i+1] + Y_over_D * h_top[i];
|
||||
in_bottom = zero_acc_bottom[i+1] + Y_over_D * h_bottom[i];
|
||||
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
|
||||
in_top, in_bottom, out_top, out_bottom);
|
||||
zero_acc_top[parent+1] += out_top;
|
||||
zero_acc_bottom[parent+1] += out_bottom;
|
||||
}
|
||||
|
||||
// ptr to the joint accel part of the output
|
||||
btScalar * joint_accel = output + 6;
|
||||
|
||||
// Second 'upward' loop
|
||||
if (fixed_base)
|
||||
{
|
||||
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
|
||||
} else
|
||||
{
|
||||
btVector3 rhs_top (zero_acc_top[0][0], zero_acc_top[0][1], zero_acc_top[0][2]);
|
||||
btVector3 rhs_bot (zero_acc_bottom[0][0], zero_acc_bottom[0][1], zero_acc_bottom[0][2]);
|
||||
|
||||
float result[6];
|
||||
solveImatrix(rhs_top,rhs_bot, result);
|
||||
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
accel_top[0][i] = -result[i];
|
||||
accel_bottom[0][i] = -result[i+3];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// now do the loop over the links
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
const int parent = links[i].parent;
|
||||
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
|
||||
accel_top[parent+1], accel_bottom[parent+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];
|
||||
accel_top[i+1] += joint_accel[i] * links[i].axis_top;
|
||||
accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom;
|
||||
}
|
||||
|
||||
// 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];
|
||||
}
|
||||
|
||||
void btMultiBody::stepPositions(btScalar dt)
|
||||
{
|
||||
// step position by adding dt * velocity
|
||||
btVector3 v = getBaseVel();
|
||||
base_pos += dt * v;
|
||||
|
||||
// "exponential map" method for the rotation
|
||||
btVector3 base_omega = getBaseOmega();
|
||||
const btScalar omega_norm = base_omega.norm();
|
||||
const btScalar omega_times_dt = omega_norm * dt;
|
||||
const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
|
||||
if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
|
||||
{
|
||||
const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2
|
||||
const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega|
|
||||
const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|)
|
||||
base_quat = base_quat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
|
||||
} else
|
||||
{
|
||||
base_quat = base_quat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
|
||||
}
|
||||
|
||||
// Make sure the quaternion represents a valid rotation.
|
||||
// (Not strictly necessary, but helps prevent any round-off errors from building up.)
|
||||
base_quat.normalize();
|
||||
|
||||
// Finally we can update joint_pos for each of the links
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
float jointVel = getJointVel(i);
|
||||
links[i].joint_pos += dt * jointVel;
|
||||
links[i].updateCache();
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBody::fillContactJacobian(int link,
|
||||
const btVector3 &contact_point,
|
||||
const btVector3 &normal,
|
||||
btScalar *jac,
|
||||
btAlignedObjectArray<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
|
||||
{
|
||||
// temporary space
|
||||
scratch_v.resize(2*num_links + 2);
|
||||
scratch_m.resize(num_links + 1);
|
||||
|
||||
btVector3 * v_ptr = &scratch_v[0];
|
||||
btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
|
||||
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
|
||||
|
||||
scratch_r.resize(num_links);
|
||||
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
|
||||
|
||||
btMatrix3x3 * rot_from_world = &scratch_m[0];
|
||||
|
||||
const btVector3 p_minus_com_world = contact_point - base_pos;
|
||||
|
||||
rot_from_world[0] = btMatrix3x3(base_quat);
|
||||
|
||||
p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
|
||||
n_local[0] = rot_from_world[0] * normal;
|
||||
|
||||
// omega coeffients first.
|
||||
btVector3 omega_coeffs;
|
||||
omega_coeffs = p_minus_com_world.cross(normal);
|
||||
jac[0] = omega_coeffs[0];
|
||||
jac[1] = omega_coeffs[1];
|
||||
jac[2] = omega_coeffs[2];
|
||||
// then v coefficients
|
||||
jac[3] = normal[0];
|
||||
jac[4] = normal[1];
|
||||
jac[5] = normal[2];
|
||||
|
||||
// Set remaining jac values to zero for now.
|
||||
for (int i = 6; i < 6 + num_links; ++i) {
|
||||
jac[i] = 0;
|
||||
}
|
||||
|
||||
// Qdot coefficients, if necessary.
|
||||
if (num_links > 0 && link > -1) {
|
||||
|
||||
// TODO: speed this up -- don't calculate for links we don't need.
|
||||
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
|
||||
// which is resulting in repeated work being done...)
|
||||
|
||||
// calculate required normals & positions in the local frames.
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
|
||||
// transform to local frame
|
||||
const int parent = links[i].parent;
|
||||
const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this);
|
||||
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
|
||||
|
||||
n_local[i+1] = mtx * n_local[parent+1];
|
||||
p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector;
|
||||
|
||||
// calculate the jacobian entry
|
||||
if (links[i].is_revolute) {
|
||||
results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom );
|
||||
} else {
|
||||
results[i] = n_local[i+1].dot( links[i].axis_bottom );
|
||||
}
|
||||
}
|
||||
|
||||
// Now copy through to output.
|
||||
while (link != -1) {
|
||||
jac[6 + link] = results[link];
|
||||
link = links[link].parent;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBody::wakeUp()
|
||||
{
|
||||
awake = true;
|
||||
}
|
||||
|
||||
void btMultiBody::goToSleep()
|
||||
{
|
||||
awake = false;
|
||||
}
|
||||
|
||||
void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
|
||||
{
|
||||
if (!can_sleep) return;
|
||||
|
||||
// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
|
||||
btScalar motion = 0;
|
||||
for (int i = 0; i < 6 + num_links; ++i) {
|
||||
motion += real_buf[i] * real_buf[i];
|
||||
}
|
||||
|
||||
if (motion < SLEEP_EPSILON) {
|
||||
sleep_timer += timestep;
|
||||
if (sleep_timer > SLEEP_TIMEOUT) {
|
||||
goToSleep();
|
||||
}
|
||||
} else {
|
||||
sleep_timer = 0;
|
||||
if (!awake)
|
||||
wakeUp();
|
||||
}
|
||||
}
|
||||
353
src/BulletDynamics/Featherstone/btMultiBody.h
Normal file
353
src/BulletDynamics/Featherstone/btMultiBody.h
Normal file
@@ -0,0 +1,353 @@
|
||||
/*
|
||||
* PURPOSE:
|
||||
* Class representing an articulated rigid body. Stores the body's
|
||||
* current state, allows forces and torques to be set, handles
|
||||
* timestepping and implements Featherstone's algorithm.
|
||||
*
|
||||
* COPYRIGHT:
|
||||
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
|
||||
* Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
|
||||
|
||||
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_H
|
||||
#define BT_MULTIBODY_H
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btQuaternion.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
|
||||
#include "btMultibodyLink.h"
|
||||
class btMultiBodyLinkCollider;
|
||||
|
||||
class btMultiBody
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
//
|
||||
// initialization
|
||||
//
|
||||
|
||||
btMultiBody(int n_links, // NOT including the base
|
||||
btScalar mass, // mass of base
|
||||
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
|
||||
bool fixed_base_, // whether the base is fixed (true) or can move (false)
|
||||
bool can_sleep_);
|
||||
|
||||
~btMultiBody();
|
||||
|
||||
void setupPrismatic(int i, // 0 to num_links-1
|
||||
btScalar mass,
|
||||
const btVector3 &inertia, // in my frame; assumed diagonal
|
||||
int parent,
|
||||
const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame.
|
||||
const btVector3 &joint_axis, // in my frame
|
||||
const btVector3 &r_vector_when_q_zero); // vector from parent COM to my COM, in my frame, when q = 0.
|
||||
|
||||
void setupRevolute(int i, // 0 to num_links-1
|
||||
btScalar mass,
|
||||
const btVector3 &inertia,
|
||||
int parent,
|
||||
const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0
|
||||
const btVector3 &joint_axis, // in my frame
|
||||
const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame
|
||||
const btVector3 &my_axis_position); // vector from joint axis to my COM, in MY frame
|
||||
|
||||
void addLinkCollider(btMultiBodyLinkCollider* collider);
|
||||
btMultiBodyLinkCollider* getLinkCollider(int index);
|
||||
const btMultiBodyLinkCollider* getLinkCollider(int index) const;
|
||||
int getNumLinkColliders() const;
|
||||
//
|
||||
// get parent
|
||||
// input: link num from 0 to num_links-1
|
||||
// output: link num from 0 to num_links-1, OR -1 to mean the base.
|
||||
//
|
||||
int getParent(int link_num) const;
|
||||
|
||||
|
||||
//
|
||||
// get number of links, masses, moments of inertia
|
||||
//
|
||||
|
||||
int getNumLinks() const { return num_links; }
|
||||
btScalar getBaseMass() const { return base_mass; }
|
||||
const btVector3 & getBaseInertia() const { return base_inertia; }
|
||||
btScalar getLinkMass(int i) const;
|
||||
const btVector3 & getLinkInertia(int i) const;
|
||||
|
||||
|
||||
//
|
||||
// change mass (incomplete: can only change base mass and inertia at present)
|
||||
//
|
||||
|
||||
void setBaseMass(btScalar mass) { base_mass = mass; }
|
||||
void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }
|
||||
|
||||
|
||||
//
|
||||
// get/set pos/vel/rot/omega for the base link
|
||||
//
|
||||
|
||||
const btVector3 & getBasePos() const { return base_pos; } // in world frame
|
||||
btVector3 getBaseVel() const {
|
||||
return btVector3(real_buf[3],real_buf[4],real_buf[5]);
|
||||
} // in world frame
|
||||
const btQuaternion & getWorldToBaseRot() const
|
||||
{
|
||||
return base_quat;
|
||||
} // rotates world vectors into base frame
|
||||
btVector3 getBaseOmega() const { return btVector3(real_buf[0],real_buf[1],real_buf[2]); } // in world frame
|
||||
|
||||
void setBasePos(const btVector3 &pos)
|
||||
{
|
||||
base_pos = pos;
|
||||
}
|
||||
void setBaseVel(const btVector3 &vel)
|
||||
{
|
||||
real_buf[3]=vel[0]; real_buf[4]=vel[1]; real_buf[5]=vel[2];
|
||||
}
|
||||
void setWorldToBaseRot(const btQuaternion &rot)
|
||||
{
|
||||
base_quat = rot;
|
||||
}
|
||||
void setBaseOmega(const btVector3 &omega) { real_buf[0]=omega[0]; real_buf[1]=omega[1]; real_buf[2]=omega[2]; }
|
||||
|
||||
|
||||
//
|
||||
// get/set pos/vel for child links (i = 0 to num_links-1)
|
||||
//
|
||||
|
||||
btScalar getJointPos(int i) const;
|
||||
btScalar getJointVel(int i) const;
|
||||
|
||||
void setJointPos(int i, btScalar q);
|
||||
void setJointVel(int i, btScalar qdot);
|
||||
|
||||
//
|
||||
// direct access to velocities as a vector of 6 + num_links elements.
|
||||
// (omega first, then v, then joint velocities.)
|
||||
//
|
||||
const btScalar * getVelocityVector() const
|
||||
{
|
||||
return &real_buf[0];
|
||||
}
|
||||
btScalar * getVelocityVector()
|
||||
{
|
||||
return &real_buf[0];
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// get the frames of reference (positions and orientations) of the child links
|
||||
// (i = 0 to num_links-1)
|
||||
//
|
||||
|
||||
const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
|
||||
const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
|
||||
|
||||
|
||||
//
|
||||
// transform vectors in local frame of link i to world frame (or vice versa)
|
||||
//
|
||||
btVector3 localPosToWorld(int i, const btVector3 &vec) const;
|
||||
btVector3 localDirToWorld(int i, const btVector3 &vec) const;
|
||||
btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
|
||||
btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
|
||||
|
||||
|
||||
//
|
||||
// calculate kinetic energy and angular momentum
|
||||
// useful for debugging.
|
||||
//
|
||||
|
||||
btScalar getKineticEnergy() const;
|
||||
btVector3 getAngularMomentum() const;
|
||||
|
||||
|
||||
//
|
||||
// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
|
||||
//
|
||||
|
||||
void clearForcesAndTorques();
|
||||
void clearVelocities();
|
||||
|
||||
void addBaseForce(const btVector3 &f)
|
||||
{
|
||||
base_force += f;
|
||||
}
|
||||
void addBaseTorque(const btVector3 &t) { base_torque += t; }
|
||||
void addLinkForce(int i, const btVector3 &f);
|
||||
void addLinkTorque(int i, const btVector3 &t);
|
||||
void addJointTorque(int i, btScalar Q);
|
||||
|
||||
const btVector3 & getBaseForce() const { return base_force; }
|
||||
const btVector3 & getBaseTorque() const { return base_torque; }
|
||||
const btVector3 & getLinkForce(int i) const;
|
||||
const btVector3 & getLinkTorque(int i) const;
|
||||
btScalar getJointTorque(int i) const;
|
||||
|
||||
|
||||
//
|
||||
// dynamics routines.
|
||||
//
|
||||
|
||||
// timestep the velocities (given the external forces/torques set using addBaseForce etc).
|
||||
// also sets up caches for calcAccelerationDeltas.
|
||||
//
|
||||
// Note: the caller must provide three vectors which are used as
|
||||
// temporary scratch space. The idea here is to reduce dynamic
|
||||
// memory allocation: the same scratch vectors can be re-used
|
||||
// again and again for different Multibodies, instead of each
|
||||
// btMultiBody allocating (and then deallocating) their own
|
||||
// individual scratch buffers. This gives a considerable speed
|
||||
// improvement, at least on Windows (where dynamic memory
|
||||
// allocation appears to be fairly slow).
|
||||
//
|
||||
void stepVelocities(btScalar dt,
|
||||
btAlignedObjectArray<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &scratch_m);
|
||||
|
||||
// calcAccelerationDeltas
|
||||
// input: force vector (in same format as jacobian, i.e.:
|
||||
// 3 torque values, 3 force values, num_links joint torque values)
|
||||
// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
|
||||
// (existing contents of output array are replaced)
|
||||
// stepVelocities must have been called first.
|
||||
void calcAccelerationDeltas(const btScalar *force, btScalar *output,
|
||||
btAlignedObjectArray<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v) const;
|
||||
|
||||
// apply a delta-vee directly. used in sequential impulses code.
|
||||
void applyDeltaVee(const btScalar * delta_vee)
|
||||
{
|
||||
for (int i = 0; i < 6 + num_links; ++i)
|
||||
real_buf[i] += delta_vee[i];
|
||||
}
|
||||
void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier)
|
||||
{
|
||||
for (int i = 0; i < 6 + num_links; ++i)
|
||||
real_buf[i] += delta_vee[i] * multiplier;
|
||||
}
|
||||
|
||||
// timestep the positions (given current velocities).
|
||||
void stepPositions(btScalar dt);
|
||||
|
||||
|
||||
//
|
||||
// contacts
|
||||
//
|
||||
|
||||
// This routine fills out a contact constraint jacobian for this body.
|
||||
// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
|
||||
// 'normal' & 'contact_point' are both given in world coordinates.
|
||||
void fillContactJacobian(int link,
|
||||
const btVector3 &contact_point,
|
||||
const btVector3 &normal,
|
||||
btScalar *jac,
|
||||
btAlignedObjectArray<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
|
||||
|
||||
|
||||
//
|
||||
// sleeping
|
||||
//
|
||||
|
||||
bool isAwake() const { return awake; }
|
||||
void wakeUp();
|
||||
void goToSleep();
|
||||
void checkMotionAndSleepIfRequired(btScalar timestep);
|
||||
|
||||
bool hasFixedBase() const
|
||||
{
|
||||
return fixed_base;
|
||||
}
|
||||
|
||||
int getCompanionId() const
|
||||
{
|
||||
return m_companionId;
|
||||
}
|
||||
void setCompanionId(int id)
|
||||
{
|
||||
m_companionId = id;
|
||||
}
|
||||
private:
|
||||
btMultiBody(const btMultiBody &); // not implemented
|
||||
void operator=(const btMultiBody &); // not implemented
|
||||
|
||||
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
|
||||
|
||||
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
|
||||
|
||||
|
||||
private:
|
||||
int num_links; // includes base.
|
||||
|
||||
btVector3 base_pos; // position of COM of base (world frame)
|
||||
btQuaternion base_quat; // rotates world points into base frame
|
||||
|
||||
btScalar base_mass; // mass of the base
|
||||
btVector3 base_inertia; // inertia of the base (in local frame; diagonal)
|
||||
|
||||
btVector3 base_force; // external force applied to base. World frame.
|
||||
btVector3 base_torque; // external torque applied to base. World frame.
|
||||
|
||||
btAlignedObjectArray<btMultibodyLink> links; // array of links, excluding the base. index from 0 to num_links-2.
|
||||
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
|
||||
|
||||
//
|
||||
// real_buf:
|
||||
// offset size array
|
||||
// 0 6 + num_links v (base_omega; base_vel; joint_vels)
|
||||
// 6+num_links num_links D
|
||||
//
|
||||
// vector_buf:
|
||||
// offset size array
|
||||
// 0 num_links h_top
|
||||
// num_links num_links h_bottom
|
||||
//
|
||||
// matrix_buf:
|
||||
// offset size array
|
||||
// 0 num_links+1 rot_from_parent
|
||||
//
|
||||
|
||||
btAlignedObjectArray<btScalar> real_buf;
|
||||
btAlignedObjectArray<btVector3> vector_buf;
|
||||
btAlignedObjectArray<btMatrix3x3> matrix_buf;
|
||||
|
||||
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
|
||||
|
||||
btMatrix3x3 cached_inertia_top_left;
|
||||
btMatrix3x3 cached_inertia_top_right;
|
||||
btMatrix3x3 cached_inertia_lower_left;
|
||||
btMatrix3x3 cached_inertia_lower_right;
|
||||
|
||||
bool fixed_base;
|
||||
|
||||
// Sleep parameters.
|
||||
bool awake;
|
||||
bool can_sleep;
|
||||
btScalar sleep_timer;
|
||||
|
||||
int m_companionId;
|
||||
};
|
||||
|
||||
#endif
|
||||
694
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
Normal file
694
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
Normal file
@@ -0,0 +1,694 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "btMultiBodyConstraintSolver.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
|
||||
|
||||
|
||||
|
||||
btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
|
||||
//solve featherstone non-contact constraints
|
||||
|
||||
for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
resolveSingleConstraintRowGeneric(constraint);
|
||||
}
|
||||
|
||||
//solve featherstone normal contact
|
||||
for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
resolveSingleConstraintRowGeneric(constraint);
|
||||
}
|
||||
|
||||
//solve featherstone frictional contact
|
||||
|
||||
for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
|
||||
{
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
{
|
||||
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
|
||||
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
|
||||
//adjust friction limits here
|
||||
if (totalImpulse>btScalar(0))
|
||||
{
|
||||
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
|
||||
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
|
||||
resolveSingleConstraintRowGeneric(frictionConstraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
m_multiBodyNonContactConstraints.resize(0);
|
||||
m_multiBodyNormalContactConstraints.resize(0);
|
||||
m_multiBodyFrictionContactConstraints.resize(0);
|
||||
m_jacobians.resize(0);
|
||||
m_deltaVelocitiesUnitImpulse.resize(0);
|
||||
m_deltaVelocities.resize(0);
|
||||
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
|
||||
if (fcA)
|
||||
{
|
||||
fcA->m_multiBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
|
||||
return val;
|
||||
}
|
||||
|
||||
void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
|
||||
{
|
||||
for (int i = 0; i < ndof; ++i)
|
||||
m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
|
||||
}
|
||||
|
||||
void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
|
||||
{
|
||||
|
||||
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
|
||||
btScalar deltaVelADotn=0;
|
||||
btScalar deltaVelBDotn=0;
|
||||
btSolverBody* bodyA = 0;
|
||||
btSolverBody* bodyB = 0;
|
||||
int ndofA=0;
|
||||
int ndofB=0;
|
||||
|
||||
if (c.m_multiBodyA)
|
||||
{
|
||||
ndofA = c.m_multiBodyA->getNumLinks() + 6;
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i];
|
||||
} else
|
||||
{
|
||||
bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
|
||||
deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
|
||||
}
|
||||
|
||||
if (c.m_multiBodyB)
|
||||
{
|
||||
ndofB = c.m_multiBodyB->getNumLinks() + 6;
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i];
|
||||
} else
|
||||
{
|
||||
bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
|
||||
deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
|
||||
}
|
||||
|
||||
|
||||
deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
|
||||
deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
|
||||
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
|
||||
|
||||
if (sum < c.m_lowerLimit)
|
||||
{
|
||||
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
|
||||
c.m_appliedImpulse = c.m_lowerLimit;
|
||||
}
|
||||
else if (sum > c.m_upperLimit)
|
||||
{
|
||||
deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
|
||||
c.m_appliedImpulse = c.m_upperLimit;
|
||||
}
|
||||
else
|
||||
{
|
||||
c.m_appliedImpulse = sum;
|
||||
}
|
||||
|
||||
if (c.m_multiBodyA)
|
||||
{
|
||||
applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
|
||||
c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||
} else
|
||||
{
|
||||
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
|
||||
}
|
||||
if (c.m_multiBodyB)
|
||||
{
|
||||
applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
|
||||
c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||
} else
|
||||
{
|
||||
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
const btVector3& contactNormal,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
|
||||
btVector3 rel_pos1;
|
||||
btVector3 rel_pos2;
|
||||
|
||||
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
|
||||
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
|
||||
|
||||
const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
|
||||
btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
|
||||
btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
|
||||
|
||||
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
|
||||
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
|
||||
|
||||
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
|
||||
// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
|
||||
if (bodyA)
|
||||
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
|
||||
if (bodyB)
|
||||
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
|
||||
|
||||
relaxation = 1.f;
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
//solverConstraint.m_jacA =
|
||||
const int ndofA = multiBodyA->getNumLinks() + 6;
|
||||
|
||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
if (solverConstraint.m_deltaVelAindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelAindex = m_deltaVelocities.size();
|
||||
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
|
||||
m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA);
|
||||
}
|
||||
|
||||
solverConstraint.m_jacAindex = m_jacobians.size();
|
||||
m_jacobians.resize(m_jacobians.size()+ndofA);
|
||||
m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
||||
btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
|
||||
|
||||
float* jac1=&m_jacobians[solverConstraint.m_jacAindex];
|
||||
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, scratch_r, scratch_v, scratch_m);
|
||||
float* delta = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacAindex],delta,scratch_r, scratch_v);
|
||||
} else
|
||||
{
|
||||
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_contactNormal1 = contactNormal;
|
||||
}
|
||||
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
|
||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
if (solverConstraint.m_deltaVelBindex <0)
|
||||
{
|
||||
solverConstraint.m_deltaVelBindex = m_deltaVelocities.size();
|
||||
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
|
||||
m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB);
|
||||
}
|
||||
|
||||
solverConstraint.m_jacBindex = m_jacobians.size();
|
||||
|
||||
m_jacobians.resize(m_jacobians.size()+ndofB);
|
||||
m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
||||
btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
|
||||
|
||||
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_jacobians[solverConstraint.m_jacBindex], scratch_r, scratch_v, scratch_m);
|
||||
multiBodyB->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacBindex],&m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],scratch_r, scratch_v);
|
||||
} else
|
||||
{
|
||||
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_contactNormal2 = -contactNormal;
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
btVector3 vec;
|
||||
btScalar denom0 = 0.f;
|
||||
btScalar denom1 = 0.f;
|
||||
if (multiBodyA)
|
||||
{
|
||||
const int ndofA = multiBodyA->getNumLinks() + 6;
|
||||
btScalar* jacA = &m_jacobians[solverConstraint.m_jacAindex];
|
||||
btScalar* lambdaA = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
{
|
||||
float j = jacA[i] ;
|
||||
float l =lambdaA[i];
|
||||
denom0 += j*l;
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (rb0)
|
||||
{
|
||||
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
|
||||
denom0 = rb0->getInvMass() + contactNormal.dot(vec);
|
||||
}
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
const int ndofB = multiBodyB->getNumLinks() + 6;
|
||||
btScalar* jacB = &m_jacobians[solverConstraint.m_jacBindex];
|
||||
btScalar* lambdaB = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
{
|
||||
float j = jacB[i] ;
|
||||
float l =lambdaB[i];
|
||||
denom1 += j*l;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
if (rb1)
|
||||
{
|
||||
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
|
||||
denom1 = rb1->getInvMass() + contactNormal.dot(vec);
|
||||
}
|
||||
}
|
||||
|
||||
btScalar denom = relaxation/(denom0+denom1);
|
||||
solverConstraint.m_jacDiagABInv = denom;
|
||||
}
|
||||
|
||||
|
||||
//compute rhs and remaining solverConstraint fields
|
||||
|
||||
|
||||
|
||||
btScalar restitution = 0.f;
|
||||
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
|
||||
|
||||
btScalar rel_vel = 0.f;
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
{
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
if (multiBodyA)
|
||||
{
|
||||
ndofA = multiBodyA->getNumLinks() + 6;
|
||||
btScalar* jacA = &m_jacobians[solverConstraint.m_jacAindex];
|
||||
for (int i = 0; i < ndofA ; ++i)
|
||||
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
|
||||
} else
|
||||
{
|
||||
if (rb0)
|
||||
{
|
||||
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
|
||||
}
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
ndofB = multiBodyB->getNumLinks() + 6;
|
||||
btScalar* jacB = &m_jacobians[solverConstraint.m_jacBindex];
|
||||
for (int i = 0; i < ndofB ; ++i)
|
||||
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
||||
|
||||
} else
|
||||
{
|
||||
if (rb1)
|
||||
{
|
||||
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
|
||||
}
|
||||
}
|
||||
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
|
||||
|
||||
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
||||
if (restitution <= btScalar(0.))
|
||||
{
|
||||
restitution = 0.f;
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
///warm starting (or zero if disabled)
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
||||
|
||||
if (solverConstraint.m_appliedImpulse)
|
||||
{
|
||||
if (multiBodyA)
|
||||
{
|
||||
btScalar impulse = solverConstraint.m_appliedImpulse;
|
||||
btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
|
||||
multiBodyA->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
|
||||
} else
|
||||
{
|
||||
if (rb0)
|
||||
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
|
||||
}
|
||||
if (multiBodyB)
|
||||
{
|
||||
btScalar impulse = solverConstraint.m_appliedImpulse;
|
||||
btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
|
||||
multiBodyB->applyDeltaVee(deltaV,impulse);
|
||||
applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
|
||||
} else
|
||||
{
|
||||
if (rb1)
|
||||
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
}
|
||||
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = restitution - rel_vel;// * damping;
|
||||
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
|
||||
// const btScalar ALLOWED_PENETRATION = btScalar(0.01);
|
||||
|
||||
// float baumgarte_coeff = 0.3;
|
||||
/// float one_over_dt = 1.f/infoGlobal.m_timeStep;
|
||||
// btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt;
|
||||
// float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
|
||||
|
||||
// solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
|
||||
//solverConstraint.m_rhsPenetration = 0.f;
|
||||
|
||||
//penetration=0.f;
|
||||
#if 1
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
velocityError = -penetration / infoGlobal.m_timeStep;
|
||||
|
||||
} else
|
||||
{
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
}
|
||||
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
|
||||
//solverConstraint.m_rhs = velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
|
||||
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = penetrationImpulse;
|
||||
}
|
||||
#endif
|
||||
|
||||
solverConstraint.m_cfm = 0.f;
|
||||
solverConstraint.m_lowerLimit = 0;
|
||||
solverConstraint.m_upperLimit = 1e10f;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
|
||||
solverConstraint.m_frictionIndex = frictionIndex;
|
||||
bool isFriction = true;
|
||||
|
||||
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
|
||||
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
|
||||
|
||||
btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
|
||||
btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
|
||||
|
||||
int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0);
|
||||
int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1);
|
||||
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
solverConstraint.m_multiBodyA = mbA;
|
||||
if (mbA)
|
||||
solverConstraint.m_linkA = fcA->m_link;
|
||||
|
||||
solverConstraint.m_multiBodyB = mbB;
|
||||
if (mbB)
|
||||
solverConstraint.m_linkB = fcB->m_link;
|
||||
|
||||
solverConstraint.m_originalContactPoint = &cp;
|
||||
|
||||
setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
|
||||
return solverConstraint;
|
||||
}
|
||||
|
||||
void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
|
||||
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
|
||||
|
||||
btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
|
||||
btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
|
||||
|
||||
btCollisionObject* colObj0=0,*colObj1=0;
|
||||
|
||||
colObj0 = (btCollisionObject*)manifold->getBody0();
|
||||
colObj1 = (btCollisionObject*)manifold->getBody1();
|
||||
|
||||
int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0);
|
||||
int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1);
|
||||
|
||||
btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
|
||||
///avoid collision response between two static objects
|
||||
// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
|
||||
// return;
|
||||
|
||||
int rollingFriction=1;
|
||||
|
||||
for (int j=0;j<manifold->getNumContacts();j++)
|
||||
{
|
||||
|
||||
btManifoldPoint& cp = manifold->getContactPoint(j);
|
||||
|
||||
if (cp.getDistance() <= manifold->getContactProcessingThreshold())
|
||||
{
|
||||
|
||||
btScalar relaxation;
|
||||
|
||||
int frictionIndex = m_multiBodyNormalContactConstraints.size();
|
||||
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
|
||||
|
||||
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
||||
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
solverConstraint.m_multiBodyA = mbA;
|
||||
if (mbA)
|
||||
solverConstraint.m_linkA = fcA->m_link;
|
||||
|
||||
solverConstraint.m_multiBodyB = mbB;
|
||||
if (mbB)
|
||||
solverConstraint.m_linkB = fcB->m_link;
|
||||
|
||||
solverConstraint.m_originalContactPoint = &cp;
|
||||
|
||||
bool isFriction = false;
|
||||
setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
|
||||
|
||||
// const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
// const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
|
||||
/////setup the friction constraints
|
||||
#define ENABLE_FRICTION
|
||||
#ifdef ENABLE_FRICTION
|
||||
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
#if ROLLING_FRICTION
|
||||
btVector3 angVelA(0,0,0),angVelB(0,0,0);
|
||||
if (rb0)
|
||||
angVelA = rb0->getAngularVelocity();
|
||||
if (rb1)
|
||||
angVelB = rb1->getAngularVelocity();
|
||||
btVector3 relAngVel = angVelB-angVelA;
|
||||
|
||||
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
|
||||
{
|
||||
//only a single rollingFriction per manifold
|
||||
rollingFriction--;
|
||||
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
|
||||
{
|
||||
relAngVel.normalize();
|
||||
applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
|
||||
if (relAngVel.length()>0.001)
|
||||
addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
} else
|
||||
{
|
||||
addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
btVector3 axis0,axis1;
|
||||
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
|
||||
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
|
||||
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
|
||||
if (axis0.length()>0.001)
|
||||
addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
if (axis1.length()>0.001)
|
||||
addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
}
|
||||
}
|
||||
#endif //ROLLING_FRICTION
|
||||
|
||||
///Bullet has several options to set the friction directions
|
||||
///By default, each contact has only a single friction direction that is recomputed automatically very frame
|
||||
///based on the relative linear velocity.
|
||||
///If the relative velocity it zero, it will automatically compute a friction direction.
|
||||
|
||||
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
|
||||
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
|
||||
///
|
||||
///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
|
||||
///
|
||||
///The user can manually override the friction directions for certain contacts using a contact callback,
|
||||
///and set the cp.m_lateralFrictionInitialized to true
|
||||
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
|
||||
///this will give a conveyor belt effect
|
||||
///
|
||||
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
|
||||
{/*
|
||||
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
|
||||
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
|
||||
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
|
||||
{
|
||||
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
|
||||
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
|
||||
cp.m_lateralFrictionDir2.normalize();//??
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
}
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
} else
|
||||
*/
|
||||
{
|
||||
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
}
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
|
||||
{
|
||||
cp.m_lateralFrictionInitialized = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
|
||||
|
||||
//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
|
||||
//todo:
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
}
|
||||
|
||||
|
||||
#endif //ENABLE_FRICTION
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
btPersistentManifold* manifold = 0;
|
||||
|
||||
for (int i=0;i<numManifolds;i++)
|
||||
{
|
||||
btPersistentManifold* manifold= manifoldPtr[i];
|
||||
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
|
||||
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
|
||||
if (!fcA && !fcB)
|
||||
{
|
||||
//the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
|
||||
convertContact(manifold,infoGlobal);
|
||||
} else
|
||||
{
|
||||
convertMultiBodyContact(manifold,infoGlobal);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
#include "btMultiBodySolverConstraint.h"
|
||||
|
||||
|
||||
class btMultiBody;
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
|
||||
protected:
|
||||
|
||||
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
|
||||
|
||||
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
|
||||
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
|
||||
|
||||
|
||||
btAlignedObjectArray<btScalar> m_jacobians;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocities;
|
||||
|
||||
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
btAlignedObjectArray<btMatrix3x3> scratch_m;
|
||||
|
||||
// virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
// virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
|
||||
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
|
||||
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
const btVector3& contactNormal,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btScalar& relaxation,
|
||||
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
|
||||
|
||||
void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
// virtual btScalar solveGroupCacheFriendlyIterations(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);
|
||||
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
|
||||
|
||||
265
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
Normal file
265
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
Normal file
@@ -0,0 +1,265 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "btMultiBodyDynamicsWorld.h"
|
||||
#include "btMultiBodyConstraintSolver.h"
|
||||
#include "btMultiBody.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
|
||||
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
|
||||
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
|
||||
{
|
||||
//split impulse is not yet supported for Featherstone hierarchies
|
||||
getSolverInfo().m_splitImpulse = false;
|
||||
getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
|
||||
}
|
||||
|
||||
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
|
||||
{
|
||||
m_multiBodies.push_back(body);
|
||||
|
||||
}
|
||||
|
||||
void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
|
||||
{
|
||||
m_multiBodies.remove(body);
|
||||
}
|
||||
|
||||
void btMultiBodyDynamicsWorld::calculateSimulationIslands()
|
||||
{
|
||||
BT_PROFILE("calculateSimulationIslands");
|
||||
|
||||
getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
|
||||
|
||||
{
|
||||
//merge islands based on speculative contact manifolds too
|
||||
for (int i=0;i<this->m_predictiveManifolds.size();i++)
|
||||
{
|
||||
btPersistentManifold* manifold = m_predictiveManifolds[i];
|
||||
|
||||
const btCollisionObject* colObj0 = manifold->getBody0();
|
||||
const btCollisionObject* colObj1 = manifold->getBody1();
|
||||
|
||||
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
|
||||
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
|
||||
{
|
||||
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int i;
|
||||
int numConstraints = int(m_constraints.size());
|
||||
for (i=0;i< numConstraints ; i++ )
|
||||
{
|
||||
btTypedConstraint* constraint = m_constraints[i];
|
||||
if (constraint->isEnabled())
|
||||
{
|
||||
const btRigidBody* colObj0 = &constraint->getRigidBodyA();
|
||||
const btRigidBody* colObj1 = &constraint->getRigidBodyB();
|
||||
|
||||
if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
|
||||
((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
|
||||
{
|
||||
getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//merge islands linked by Featherstone links
|
||||
for (int i=0;i<m_multiBodies.size();i++)
|
||||
{
|
||||
btMultiBody* body = m_multiBodies[i];
|
||||
if (body->getNumLinkColliders()>1)
|
||||
{
|
||||
btMultiBodyLinkCollider* prev = body->getLinkCollider(0);
|
||||
for (int b=1;b<body->getNumLinkColliders();b++)
|
||||
{
|
||||
btMultiBodyLinkCollider* cur = body->getLinkCollider(b);
|
||||
|
||||
if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
|
||||
((prev) && (!(prev)->isStaticOrKinematicObject())))
|
||||
{
|
||||
int tagPrev = prev->getIslandTag();
|
||||
int tagCur = cur->getIslandTag();
|
||||
getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
|
||||
}
|
||||
prev = cur;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//Store the island id in each body
|
||||
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
|
||||
{
|
||||
BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
|
||||
|
||||
|
||||
|
||||
for ( int i=0;i<m_multiBodies.size();i++)
|
||||
{
|
||||
btMultiBody* body = m_multiBodies[i];
|
||||
if (body)
|
||||
{
|
||||
body->checkMotionAndSleepIfRequired(timeStep);
|
||||
if (!body->isAwake())
|
||||
{
|
||||
for (int b=0;b<body->getNumLinkColliders();b++)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = body->getLinkCollider(b);
|
||||
if (col->getActivationState() == ACTIVE_TAG)
|
||||
{
|
||||
col->setActivationState( WANTS_DEACTIVATION);
|
||||
col->setDeactivationTime(0.f);
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
for (int b=0;b<body->getNumLinkColliders();b++)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = body->getLinkCollider(b);
|
||||
if (col->getActivationState() != DISABLE_DEACTIVATION)
|
||||
col->setActivationState( ACTIVE_TAG );
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
btDiscreteDynamicsWorld::updateActivationState(timeStep);
|
||||
}
|
||||
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
btVector3 g = m_gravity;
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
btAlignedObjectArray<btMatrix3x3> scratch_m;
|
||||
|
||||
|
||||
{
|
||||
BT_PROFILE("btMultiBody addForce and stepVelocities");
|
||||
for (int i=0;i<this->m_multiBodies.size();i++)
|
||||
{
|
||||
btMultiBody* bod = m_multiBodies[i];
|
||||
|
||||
bool isSleeping = false;
|
||||
if (bod->getNumLinkColliders() && bod->getLinkCollider(0)->getActivationState()==ISLAND_SLEEPING)
|
||||
{
|
||||
isSleeping = true;
|
||||
}
|
||||
if (!isSleeping)
|
||||
{
|
||||
scratch_r.resize(bod->getNumLinks()+1);
|
||||
scratch_v.resize(bod->getNumLinks()+1);
|
||||
scratch_m.resize(bod->getNumLinks()+1);
|
||||
|
||||
bod->clearForcesAndTorques();
|
||||
bod->addBaseForce(g * bod->getBaseMass());
|
||||
|
||||
for (int j = 0; j < bod->getNumLinks(); ++j)
|
||||
{
|
||||
bod->addLinkForce(j, g * bod->getLinkMass(j));
|
||||
}
|
||||
|
||||
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
|
||||
}
|
||||
}
|
||||
}
|
||||
btDiscreteDynamicsWorld::solveConstraints(solverInfo);
|
||||
}
|
||||
|
||||
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
|
||||
{
|
||||
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
|
||||
|
||||
{
|
||||
BT_PROFILE("btMultiBody stepPositions");
|
||||
//integrate and update the Featherstone hierarchies
|
||||
btAlignedObjectArray<btQuaternion> world_to_local;
|
||||
btAlignedObjectArray<btVector3> local_origin;
|
||||
|
||||
for (int b=0;b<m_multiBodies.size();b++)
|
||||
{
|
||||
btMultiBody* bod = m_multiBodies[b];
|
||||
bool isSleeping = false;
|
||||
if (bod->getNumLinkColliders() && bod->getLinkCollider(0)->getActivationState()==ISLAND_SLEEPING)
|
||||
{
|
||||
isSleeping = true;
|
||||
}
|
||||
if (!isSleeping)
|
||||
{
|
||||
int nLinks = bod->getNumLinks();
|
||||
|
||||
///base + num links
|
||||
world_to_local.resize(nLinks+1);
|
||||
local_origin.resize(nLinks+1);
|
||||
|
||||
bod->stepPositions(timeStep);
|
||||
|
||||
|
||||
|
||||
world_to_local[0] = bod->getWorldToBaseRot();
|
||||
local_origin[0] = bod->getBasePos();
|
||||
|
||||
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->getNumLinkColliders();m++)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = bod->getLinkCollider(m);
|
||||
int link = col->m_link;
|
||||
int index = link+1;
|
||||
|
||||
float halfExtents[3]={7.5,0.05,4.5};
|
||||
btVector3 posr = local_origin[index];
|
||||
float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
||||
|
||||
float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
|
||||
//app->drawBox(halfExtents, pos,quat);
|
||||
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(posr);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
|
||||
col->setWorldTransform(tr);
|
||||
}
|
||||
} else
|
||||
{
|
||||
bod->clearVelocities();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
48
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
Normal file
48
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
Normal file
@@ -0,0 +1,48 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
#define BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
|
||||
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
|
||||
|
||||
|
||||
class btMultiBody;
|
||||
class btMultiBodyConstraintSolver;
|
||||
|
||||
///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
|
||||
///This implementation is still preliminary/experimental.
|
||||
class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
|
||||
{
|
||||
protected:
|
||||
btAlignedObjectArray<btMultiBody*> m_multiBodies;
|
||||
|
||||
virtual void calculateSimulationIslands();
|
||||
virtual void updateActivationState(btScalar timeStep);
|
||||
virtual void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
public:
|
||||
|
||||
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
|
||||
|
||||
virtual ~btMultiBodyDynamicsWorld ();
|
||||
|
||||
virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
|
||||
|
||||
virtual void removeMultiBody(btMultiBody* body);
|
||||
|
||||
|
||||
};
|
||||
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
102
src/BulletDynamics/Featherstone/btMultiBodyLink.h
Normal file
102
src/BulletDynamics/Featherstone/btMultiBodyLink.h
Normal file
@@ -0,0 +1,102 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_LINK_H
|
||||
#define BT_MULTIBODY_LINK_H
|
||||
|
||||
#include "LinearMath/btQuaternion.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
|
||||
//
|
||||
// Link struct
|
||||
//
|
||||
|
||||
struct btMultibodyLink
|
||||
{
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btScalar joint_pos; // qi
|
||||
|
||||
btScalar mass; // mass of link
|
||||
btVector3 inertia; // inertia of link (local frame; diagonal)
|
||||
|
||||
int parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
|
||||
|
||||
btQuaternion zero_rot_parent_to_this; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
|
||||
|
||||
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
|
||||
// for prismatic: axis_top = zero;
|
||||
// axis_bottom = unit vector along the joint axis.
|
||||
// for revolute: axis_top = unit vector along the rotation axis (u);
|
||||
// axis_bottom = u cross d_vector.
|
||||
btVector3 axis_top;
|
||||
btVector3 axis_bottom;
|
||||
|
||||
btVector3 d_vector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only.
|
||||
|
||||
// e_vector is constant, but depends on the joint type
|
||||
// prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
|
||||
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
|
||||
btVector3 e_vector;
|
||||
|
||||
bool is_revolute; // true = revolute, false = prismatic
|
||||
|
||||
btQuaternion cached_rot_parent_to_this; // rotates vectors in parent frame to vectors in local frame
|
||||
btVector3 cached_r_vector; // vector from COM of parent to COM of this link, in local frame.
|
||||
|
||||
btVector3 applied_force; // In WORLD frame
|
||||
btVector3 applied_torque; // In WORLD frame
|
||||
btScalar joint_torque;
|
||||
|
||||
// ctor: set some sensible defaults
|
||||
btMultibodyLink()
|
||||
: joint_pos(0),
|
||||
mass(1),
|
||||
parent(-1),
|
||||
zero_rot_parent_to_this(1, 0, 0, 0),
|
||||
is_revolute(false),
|
||||
cached_rot_parent_to_this(1, 0, 0, 0),
|
||||
joint_torque(0)
|
||||
{
|
||||
inertia.setValue(1, 1, 1);
|
||||
axis_top.setValue(0, 0, 0);
|
||||
axis_bottom.setValue(1, 0, 0);
|
||||
d_vector.setValue(0, 0, 0);
|
||||
e_vector.setValue(0, 0, 0);
|
||||
cached_r_vector.setValue(0, 0, 0);
|
||||
applied_force.setValue( 0, 0, 0);
|
||||
applied_torque.setValue(0, 0, 0);
|
||||
}
|
||||
|
||||
// routine to update cached_rot_parent_to_this and cached_r_vector
|
||||
void updateCache()
|
||||
{
|
||||
if (is_revolute)
|
||||
{
|
||||
cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this;
|
||||
cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector);
|
||||
} else
|
||||
{
|
||||
// cached_rot_parent_to_this never changes, so no need to update
|
||||
cached_r_vector = e_vector + joint_pos * axis_bottom;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_MULTIBODY_LINK_H
|
||||
62
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
Normal file
62
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
Normal file
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
#define BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
#include "btMultiBody.h"
|
||||
|
||||
class btMultiBodyLinkCollider : public btCollisionObject
|
||||
{
|
||||
//protected:
|
||||
public:
|
||||
|
||||
btMultiBody* m_multiBody;
|
||||
int m_link;
|
||||
|
||||
|
||||
btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
|
||||
:m_multiBody(multiBody),
|
||||
m_link(link)
|
||||
{
|
||||
if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
|
||||
{
|
||||
m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
|
||||
} else
|
||||
{
|
||||
m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
|
||||
}
|
||||
|
||||
m_internalType = CO_FEATHERSTONE_LINK;
|
||||
}
|
||||
static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
|
||||
{
|
||||
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
|
||||
return (btMultiBodyLinkCollider*)colObj;
|
||||
return 0;
|
||||
}
|
||||
static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
|
||||
{
|
||||
if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
|
||||
return (btMultiBodyLinkCollider*)colObj;
|
||||
return 0;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
class btMultiBody;
|
||||
|
||||
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
|
||||
ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
|
||||
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
|
||||
btVector3 m_relpos1CrossNormal;
|
||||
btVector3 m_contactNormal1;
|
||||
int m_jacAindex;
|
||||
|
||||
int m_deltaVelBindex;
|
||||
btVector3 m_relpos2CrossNormal;
|
||||
btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
|
||||
int m_jacBindex;
|
||||
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
mutable btSimdScalar m_appliedPushImpulse;
|
||||
mutable btSimdScalar m_appliedImpulse;
|
||||
|
||||
btScalar m_friction;
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_rhs;
|
||||
btScalar m_cfm;
|
||||
|
||||
btScalar m_lowerLimit;
|
||||
btScalar m_upperLimit;
|
||||
btScalar m_rhsPenetration;
|
||||
union
|
||||
{
|
||||
void* m_originalContactPoint;
|
||||
btScalar m_unusedPadding4;
|
||||
};
|
||||
|
||||
int m_overrideNumSolverIterations;
|
||||
int m_frictionIndex;
|
||||
|
||||
int m_solverBodyIdA;
|
||||
btMultiBody* m_multiBodyA;
|
||||
int m_linkA;
|
||||
|
||||
int m_solverBodyIdB;
|
||||
btMultiBody* m_multiBodyB;
|
||||
int m_linkB;
|
||||
|
||||
enum btSolverConstraintType
|
||||
{
|
||||
BT_SOLVER_CONTACT_1D = 0,
|
||||
BT_SOLVER_FRICTION_1D
|
||||
};
|
||||
};
|
||||
|
||||
typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
|
||||
|
||||
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H
|
||||
Reference in New Issue
Block a user