diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone')
17 files changed, 4482 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp new file mode 100644 index 00000000000..56a1c55d9ae --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -0,0 +1,1009 @@ +/* + * 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.05); // 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_) + : 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), + m_baseCollider(0), + m_linearDamping(0.04f), + m_angularDamping(0.04f), + m_useGyroTerm(true), + m_maxAppliedImpulse(1000.f), + m_hasSelfCollision(true) +{ + links.resize(n_links); + + vector_buf.resize(2*n_links); + matrix_buf.resize(n_links + 1); + m_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, + bool disableParentCollision) +{ + 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; + if (disableParentCollision) + links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + + 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, + bool disableParentCollision) +{ + 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; + if (disableParentCollision) + links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + links[i].updateCache(); +} + + + + + +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 m_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) +{ + m_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 +{ + int num_links = getNumLinks(); + // 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 +{ + int num_links = getNumLinks(); + // 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 +{ + int num_links = getNumLinks(); + // 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 < getNumLinks(); ++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 + getNumLinks(); ++i) + { + m_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. + + int num_links = getNumLinks(); + + const btScalar DAMPING_K1_LINEAR = m_linearDamping; + const btScalar DAMPING_K2_LINEAR = m_linearDamping; + + const btScalar DAMPING_K1_ANGULAR = m_angularDamping; + const btScalar DAMPING_K2_ANGULAR= m_angularDamping; + + btVector3 base_vel = getBaseVel(); + btVector3 base_omega = getBaseOmega(); + + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + scratch_r.resize(2*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_angular = v_ptr; v_ptr += num_links + 1; + btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1; + + // zhat_i^A + btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; + btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; + + // chat_i (note NOT defined for the base) + btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links; + btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links; + + // top left, top right and bottom left blocks of Ihat_i^A. + // bottom right block = transpose of top left block and is not stored. + // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently. + btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1]; + btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2]; + btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3]; + + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3 * rot_from_parent = &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 ? &m_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_angular[0] = rot_from_parent[0] * base_omega; + vel_bottom_linear[0] = rot_from_parent[0] * base_vel; + + if (fixed_base) { + zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); + } else { + zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force + - base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); + + zero_acc_bottom_linear[0] = + - (rot_from_parent[0] * base_torque); + + if (m_useGyroTerm) + zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] ); + + zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[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_angular[parent+1], vel_bottom_linear[parent+1], + vel_top_angular[i+1], vel_bottom_linear[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_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector)) + + 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i); + if (links[i].is_revolute) { + coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i); + coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom); + } else { + coriolis_top_angular[i] = btVector3(0,0,0); + } + + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top; + vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom; + + // calculate zhat_i^A + zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force)); + zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; + + zero_acc_bottom_linear[i+1] = + - (rot_from_world[i+1] * links[i].applied_torque); + if (m_useGyroTerm) + { + zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] ); + } + + zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); + + // calculate Ihat_i^A + inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); + inertia_top_right[i+1].setValue(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; + btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]); + D[i] = val; + Y[i] = links[i].joint_torque + - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]) + - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[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_angular[i+1] + + inertia_top_left[i+1] * coriolis_top_angular[i] + + inertia_top_right[i+1] * coriolis_bottom_linear[i] + + Y_over_D * h_top[i]; + in_bottom = zero_acc_bottom_linear[i+1] + + inertia_bottom_left[i+1] * coriolis_top_angular[i] + + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[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_angular[parent+1] += out_top; + zero_acc_bottom_linear[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_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); + btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); + 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_angular[i] + joint_accel[i] * links[i].axis_top; + accel_bottom[i+1] += coriolis_bottom_linear[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 +{ + int num_links = getNumLinks(); + ///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 + int num_links = getNumLinks(); + 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_angular = v_ptr; v_ptr += num_links + 1; + btVector3 * zero_acc_bottom_linear = 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 ? &m_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_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); + } else + { + zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force); + zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque); + } + for (int i = 0; i < num_links; ++i) + { + zero_acc_top_angular[i+1] = zero_acc_bottom_linear[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_angular[i+1], zero_acc_bottom_linear[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_angular[i+1] + Y_over_D * h_top[i]; + in_bottom = zero_acc_bottom_linear[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_angular[parent+1] += out_top; + zero_acc_bottom_linear[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_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); + btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); + + 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) +{ + int num_links = getNumLinks(); + // 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 + int num_links = getNumLinks(); + 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) +{ + int num_links = getNumLinks(); + extern bool gDisableDeactivation; + if (!can_sleep || gDisableDeactivation) + { + awake = true; + sleep_timer = 0; + 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 += m_real_buf[i] * m_real_buf[i]; + } + + if (motion < SLEEP_EPSILON) { + sleep_timer += timestep; + if (sleep_timer > SLEEP_TIMEOUT) { + goToSleep(); + } + } else { + sleep_timer = 0; + if (!awake) + wakeUp(); + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h new file mode 100644 index 00000000000..7177bebbff5 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h @@ -0,0 +1,466 @@ +/* + * 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. + bool disableParentCollision=false + ); + + 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 + bool disableParentCollision=false); + + const btMultibodyLink& getLink(int index) const + { + return links[index]; + } + + btMultibodyLink& getLink(int index) + { + return links[index]; + } + + + void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base + { + m_baseCollider = collider; + } + const btMultiBodyLinkCollider* getBaseCollider() const + { + return m_baseCollider; + } + btMultiBodyLinkCollider* getBaseCollider() + { + return m_baseCollider; + } + + // + // 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 links.size(); } + 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 + const btVector3 getBaseVel() const + { + return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); + } // in world frame + const btQuaternion & getWorldToBaseRot() const + { + return base_quat; + } // rotates world vectors into base frame + btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame + + void setBasePos(const btVector3 &pos) + { + base_pos = pos; + } + void setBaseVel(const btVector3 &vel) + { + + m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; + } + void setWorldToBaseRot(const btQuaternion &rot) + { + base_quat = rot; + } + void setBaseOmega(const btVector3 &omega) + { + m_real_buf[0]=omega[0]; + m_real_buf[1]=omega[1]; + m_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 &m_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 + getNumLinks(); ++i) + { + m_real_buf[i] += delta_vee[i]; + } + + } + void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) + { + btScalar sum = 0; + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; + } + btScalar l = btSqrt(sum); + /* + static btScalar maxl = -1e30f; + if (l>maxl) + { + maxl=l; + // printf("maxl=%f\n",maxl); + } + */ + if (l>m_maxAppliedImpulse) + { +// printf("exceeds 100: l=%f\n",maxl); + multiplier *= m_maxAppliedImpulse/l; + } + + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; + m_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 + // + void setCanSleep(bool canSleep) + { + can_sleep = canSleep; + } + + 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) + { + //printf("for %p setCompanionId(%d)\n",this, id); + m_companionId = id; + } + + void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links + { + links.resize(numLinks); + } + + btScalar getLinearDamping() const + { + return m_linearDamping; + } + void setLinearDamping( btScalar damp) + { + m_linearDamping = damp; + } + btScalar getAngularDamping() const + { + return m_angularDamping; + } + + bool getUseGyroTerm() const + { + return m_useGyroTerm; + } + void setUseGyroTerm(bool useGyro) + { + m_useGyroTerm = useGyro; + } + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } + + void setHasSelfCollision(bool hasSelfCollision) + { + m_hasSelfCollision = hasSelfCollision; + } + bool hasSelfCollision() const + { + return m_hasSelfCollision; + } + +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: + + btMultiBodyLinkCollider* m_baseCollider;//can be NULL + + 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-1. + 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> m_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; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_useGyroTerm; + btScalar m_maxAppliedImpulse; + bool m_hasSelfCollision; +}; + +#endif diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp new file mode 100644 index 00000000000..44e04c3a132 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -0,0 +1,527 @@ +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) + :m_bodyA(bodyA), + m_bodyB(bodyB), + m_linkA(linkA), + m_linkB(linkB), + m_num_rows(numRows), + m_isUnilateral(isUnilateral), + m_maxAppliedImpulse(100) +{ + m_jac_size_A = (6 + bodyA->getNumLinks()); + m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0)); + m_pos_offset = ((1 + m_jac_size_both)*m_num_rows); + m_data.resize((2 + m_jac_size_both) * m_num_rows); +} + +btMultiBodyConstraint::~btMultiBodyConstraint() +{ +} + + + +btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, + btMultiBodyJacobianData& data, + btScalar* jacOrgA,btScalar* jacOrgB, + const btContactSolverInfo& infoGlobal, + btScalar desiredVelocity, + btScalar lowerLimit, + btScalar upperLimit) +{ + + + + constraintRow.m_multiBodyA = m_bodyA; + constraintRow.m_multiBodyB = m_bodyB; + + btMultiBody* multiBodyA = constraintRow.m_multiBodyA; + btMultiBody* multiBodyB = constraintRow.m_multiBodyB; + + if (multiBodyA) + { + + const int ndofA = multiBodyA->getNumLinks() + 6; + + constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (constraintRow.m_deltaVelAindex <0) + { + constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); + } + + constraintRow.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + for (int i=0;i<ndofA;i++) + data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i]; + + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (constraintRow.m_deltaVelBindex <0) + { + constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + constraintRow.m_jacBindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + + for (int i=0;i<ndofB;i++) + data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i]; + + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); + } + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + jacA = &data.m_jacobians[constraintRow.m_jacAindex]; + lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + jacB = &data.m_jacobians[constraintRow.m_jacBindex]; + lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } + + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * lambdaA[i]; + denom1 += jacA[i] * lambdaB[i]; + } + } + + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + constraintRow.m_jacDiagABInv = 1.f/(d); + } else + { + constraintRow.m_jacDiagABInv = 1.f; + } + + } + + + //compute rhs and remaining constraintRow fields + + + + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumLinks() + 6; + btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + + constraintRow.m_friction = 0.f; + + constraintRow.m_appliedImpulse = 0.f; + constraintRow.m_appliedPushImpulse = 0.f; + + btScalar velocityError = desiredVelocity - rel_vel;// * damping; + + btScalar erp = infoGlobal.m_erp2; + + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse) + { + //combine position and velocity into rhs + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + } + + + constraintRow.m_cfm = 0.f; + constraintRow.m_lowerLimit = lowerLimit; + constraintRow.m_upperLimit = upperLimit; + + } + return rel_vel; +} + + +void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + + +void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar position, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + + btVector3 rel_pos1 = posAworld; + btVector3 rel_pos2 = posBworld; + + solverConstraint.m_multiBodyA = m_bodyA; + solverConstraint.m_multiBodyB = m_bodyB; + solverConstraint.m_linkA = m_linkA; + solverConstraint.m_linkB = m_linkB; + + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = posAworld; + const btVector3& pos2 = posBworld; + + btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); + btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + const int ndofA = multiBodyA->getNumLinks() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } else + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormalOnB; + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = data.m_jacobians.size(); + + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); + } else + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormalOnB; + } + + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + } + } + + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * lambdaA[i]; + denom1 += jacA[i] * lambdaB[i]; + } + } + + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + solverConstraint.m_jacDiagABInv = 1.f; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : position+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 = &data.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 = &data.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 = 0.f;//cp.m_combinedFriction; + + + restitution = restitution * -rel_vel;//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 = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,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 = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,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; + } + + 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_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; + solverConstraint.m_upperLimit = m_maxAppliedImpulse; + } + +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h new file mode 100644 index 00000000000..9fa317330b3 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -0,0 +1,166 @@ +/* +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_H +#define BT_MULTIBODY_CONSTRAINT_H + +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btMultiBody.h" + +class btMultiBody; +struct btSolverInfo; + +#include "btMultiBodySolverConstraint.h" + +struct btMultiBodyJacobianData +{ + btAlignedObjectArray<btScalar> m_jacobians; + btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse; + btAlignedObjectArray<btScalar> m_deltaVelocities; + btAlignedObjectArray<btScalar> scratch_r; + btAlignedObjectArray<btVector3> scratch_v; + btAlignedObjectArray<btMatrix3x3> scratch_m; + btAlignedObjectArray<btSolverBody>* m_solverBodyPool; + int m_fixedBodyId; + +}; + + +class btMultiBodyConstraint +{ +protected: + + btMultiBody* m_bodyA; + btMultiBody* m_bodyB; + int m_linkA; + int m_linkB; + + int m_num_rows; + int m_jac_size_A; + int m_jac_size_both; + int m_pos_offset; + + bool m_isUnilateral; + + btScalar m_maxAppliedImpulse; + + + // data block laid out as follows: + // cached impulses. (one per row.) + // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) + // positions. (one per row.) + btAlignedObjectArray<btScalar> m_data; + + void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); + + void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar position, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, + btMultiBodyJacobianData& data, + btScalar* jacOrgA,btScalar* jacOrgB, + const btContactSolverInfo& infoGlobal, + btScalar desiredVelocity, + btScalar lowerLimit, + btScalar upperLimit); + +public: + + btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); + virtual ~btMultiBodyConstraint(); + + + + virtual int getIslandIdA() const =0; + virtual int getIslandIdB() const =0; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal)=0; + + int getNumRows() const + { + return m_num_rows; + } + + btMultiBody* getMultiBodyA() + { + return m_bodyA; + } + btMultiBody* getMultiBodyB() + { + return m_bodyB; + } + + // current constraint position + // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral + // NOTE: ignored position for friction rows. + btScalar getPosition(int row) const + { + return m_data[m_pos_offset + row]; + } + + void setPosition(int row, btScalar pos) + { + m_data[m_pos_offset + row] = pos; + } + + + bool isUnilateral() const + { + return m_isUnilateral; + } + + // jacobian blocks. + // each of size 6 + num_links. (jacobian2 is null if no body2.) + // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. + btScalar* jacobianA(int row) + { + return &m_data[m_num_rows + row * m_jac_size_both]; + } + const btScalar* jacobianA(int row) const + { + return &m_data[m_num_rows + (row * m_jac_size_both)]; + } + btScalar* jacobianB(int row) + { + return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + } + const btScalar* jacobianB(int row) const + { + return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + } + + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } + + +}; + +#endif //BT_MULTIBODY_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp new file mode 100644 index 00000000000..577f846225b --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -0,0 +1,795 @@ +/* +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" +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +#include "LinearMath/btQuickprof.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 + + //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); + for (int j=0;j<m_multiBodyNonContactConstraints.size();j++) + { + btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j]; + //if (iteration < constraint.m_overrideNumSolverIterations) + //resolveSingleConstraintRowGenericMultiBody(constraint); + 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_data.m_jacobians.resize(0); + m_data.m_deltaVelocitiesUnitImpulse.resize(0); + m_data.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_data.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_data.m_jacobians[c.m_jacAindex+i] * m_data.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_data.m_jacobians[c.m_jacBindex+i] * m_data.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_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } else + { + bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + + } + if (c.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } else + { + bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } + +} + + +void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c) +{ + + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=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_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; + } + + if (c.m_multiBodyB) + { + ndofB = c.m_multiBodyB->getNumLinks() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; + } + + + 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_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } + if (c.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } +} + + +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + BT_PROFILE("setupMultiBodyContactConstraint"); + 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; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + const int ndofA = multiBodyA->getNumLinks() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + } else + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); + 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_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + } else + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + 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; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar 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; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormal.dot(vec); + } + } + + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * lambdaA[i]; + denom1 += jacA[i] * lambdaB[i]; + } + } + + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + solverConstraint.m_jacDiagABInv = 1.f; + } + + } + + + //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_data.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_data.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_data.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_data.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; + } + + 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_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + 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) +{ + BT_PROFILE("addMultiBodyFrictionConstraint"); + 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,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + + 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,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + + 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 = frictionIndex; +#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); + } + } + + //also convert the multibody constraints, if any + + + for (int i=0;i<m_tmpNumMultiBodyConstraints;i++) + { + btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i]; + m_data.m_solverBodyPool = &m_tmpSolverBodyPool; + m_data.m_fixedBodyId = m_fixedBodyId; + + c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, 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); +} + + +void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +{ + //printf("solveMultiBodyGroup start\n"); + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; + + +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h new file mode 100644 index 00000000000..0f4cd69c029 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -0,0 +1,85 @@ +/* +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; + +#include "btMultiBodyConstraint.h" + + + +ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver +{ + +protected: + + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; + + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + + btMultiBodyJacobianData m_data; + + //temp storage for multi body constraints for a specific island/group called by 'solveGroup' + btMultiBodyConstraint** m_tmpMultiBodyConstraints; + int m_tmpNumMultiBodyConstraints; + + void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + void resolveSingleConstraintRowGenericMultiBody(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 setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, + btScalar* jacA,btScalar* jacB, + btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); + + 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(); + + ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); + + virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); +}; + + + + + +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp new file mode 100644 index 00000000000..0910f8f6abb --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -0,0 +1,578 @@ +/* +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" +#include "btMultiBodyConstraint.h" + + + + +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 link colliders + for (int i=0;i<m_multiBodies.size();i++) + { + btMultiBody* body = m_multiBodies[i]; + { + btMultiBodyLinkCollider* prev = body->getBaseCollider(); + + for (int b=0;b<body->getNumLinks();b++) + { + btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; + + if (((cur) && (!(cur)->isStaticOrKinematicObject())) && + ((prev) && (!(prev)->isStaticOrKinematicObject()))) + { + int tagPrev = prev->getIslandTag(); + int tagCur = cur->getIslandTag(); + getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); + } + if (cur && !cur->isStaticOrKinematicObject()) + prev = cur; + + } + } + } + + //merge islands linked by multibody constraints + { + for (int i=0;i<this->m_multiBodyConstraints.size();i++) + { + btMultiBodyConstraint* c = m_multiBodyConstraints[i]; + int tagA = c->getIslandIdA(); + int tagB = c->getIslandIdB(); + if (tagA>=0 && tagB>=0) + getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); + } + } + + //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()) + { + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + for (int b=0;b<body->getNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + } + } else + { + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + + for (int b=0;b<body->getNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + } + } + + } + } + + btDiscreteDynamicsWorld::updateActivationState(timeStep); +} + + +SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; + +} + + +class btSortConstraintOnIslandPredicate2 +{ + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId2(rhs); + lIslandId0 = btGetConstraintIslandId2(lhs); + return lIslandId0 < rIslandId0; + } +}; + + + +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +{ + int islandId; + + int islandTagA = lhs->getIslandIdA(); + int islandTagB = lhs->getIslandIdB(); + islandId= islandTagA>=0?islandTagA:islandTagB; + return islandId; + +} + + +class btSortMultiBodyConstraintOnIslandPredicate +{ + public: + + bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); + lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + btContactSolverInfo* m_solverInfo; + btMultiBodyConstraintSolver* m_solver; + btMultiBodyConstraint** m_multiBodySortedConstraints; + int m_numMultiBodyConstraints; + + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + + btAlignedObjectArray<btCollisionObject*> m_bodies; + btAlignedObjectArray<btPersistentManifold*> m_manifolds; + btAlignedObjectArray<btTypedConstraint*> m_constraints; + btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; + + + MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + :m_solverInfo(NULL), + m_solver(solver), + m_multiBodySortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) + { + + } + + MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + { + btAssert(solverInfo); + m_solverInfo = solverInfo; + + m_multiBodySortedConstraints = sortedMultiBodyConstraints; + m_numMultiBodyConstraints = numMultiBodyConstraints; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + + m_debugDrawer = debugDrawer; + m_bodies.resize (0); + m_manifolds.resize (0); + m_constraints.resize (0); + m_multiBodyConstraints.resize(0); + } + + + virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + { + if (islandId<0) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + btMultiBodyConstraint** startMultiBodyConstraint = 0; + + int numCurConstraints = 0; + int numCurMultiBodyConstraints = 0; + + int i; + + //find the first constraint for this island + + for (i=0;i<m_numConstraints;i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + startConstraint = &m_sortedConstraints[i]; + break; + } + } + //count the number of constraints in this island + for (;i<m_numConstraints;i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } + } + + for (i=0;i<m_numMultiBodyConstraints;i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + + startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; + break; + } + } + //count the number of multi body constraints in this island + for (;i<m_numMultiBodyConstraints;i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + numCurMultiBodyConstraints++; + } + } + + if (m_solverInfo->m_minimumSolverBatchSize<=1) + { + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + + for (i=0;i<numBodies;i++) + m_bodies.push_back(bodies[i]); + for (i=0;i<numManifolds;i++) + m_manifolds.push_back(manifolds[i]); + for (i=0;i<numCurConstraints;i++) + m_constraints.push_back(startConstraint[i]); + + for (i=0;i<numCurMultiBodyConstraints;i++) + m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); + + if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(); + } else + { + //printf("deferred\n"); + } + } + } + } + void processConstraints() + { + + btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; + btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; + btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + + m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } + +}; + + + +btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) + :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), + m_multiBodyConstraintSolver(constraintSolver) +{ + //split impulse is not yet supported for Featherstone hierarchies + getSolverInfo().m_splitImpulse = false; + getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; + m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); +} + +btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () +{ + delete m_solverMultiBodyIslandCallback; +} + + + + +void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + + btAlignedObjectArray<btScalar> scratch_r; + btAlignedObjectArray<btVector3> scratch_v; + btAlignedObjectArray<btMatrix3x3> scratch_m; + + + BT_PROFILE("solveConstraints"); + + m_sortedConstraints.resize( m_constraints.size()); + int i; + for (i=0;i<getNumConstraints();i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i=0;i<m_multiBodyConstraints.size();i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + + + m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + /// solve all the constraints for this island + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); + + + { + 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->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;b<bod->getNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + scratch_r.resize(bod->getNumLinks()+1); + scratch_v.resize(bod->getNumLinks()+1); + scratch_m.resize(bod->getNumLinks()+1); + + bod->clearForcesAndTorques(); + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + + bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + } + } + } + + m_solverMultiBodyIslandCallback->processConstraints(); + + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); + +} + +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->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;b<bod->getNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + + if (!isSleeping) + { + 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(); + + if (bod->getBaseCollider()) + { + btVector3 posr = local_origin[0]; + float pos[4]={posr.x(),posr.y(),posr.z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + bod->getBaseCollider()->setWorldTransform(tr); + + } + + for (int k=0;k<bod->getNumLinks();k++) + { + const int parent = bod->getParent(k); + world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1]; + local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); + } + + + for (int m=0;m<bod->getNumLinks();m++) + { + btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); + + int index = link+1; + + 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()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + col->setWorldTransform(tr); + } + } + } else + { + bod->clearVelocities(); + } + } + } +} + + + +void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.push_back(constraint); +} + +void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.remove(constraint); +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h new file mode 100644 index 00000000000..ad57a346dcc --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -0,0 +1,56 @@ +/* +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 btMultiBodyConstraint; +class btMultiBodyConstraintSolver; +struct MultiBodyInplaceSolverIslandCallback; + +///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet +///This implementation is still preliminary/experimental. +class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld +{ +protected: + btAlignedObjectArray<btMultiBody*> m_multiBodies; + btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; + btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints; + btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; + MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + + 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); + + virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); + + virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); +}; +#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp new file mode 100644 index 00000000000..ea309e8857d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -0,0 +1,133 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyJointLimitConstraint.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) + :btMultiBodyConstraint(body,body,link,link,2,true), + m_lowerBound(lower), + m_upperBound(upper) +{ + // the data.m_jacobians never change, so may as well + // initialize them here + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + + // row 0: the lower bound + jacobianA(0)[6 + link] = 1; + + // row 1: the upper bound + jacobianB(1)[6 + link] = -1; +} +btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() +{ +} + +int btMultiBodyJointLimitConstraint::getIslandIdA() const +{ + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;i<m_bodyA->getNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + return -1; +} + +int btMultiBodyJointLimitConstraint::getIslandIdB() const +{ + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;i<m_bodyB->getNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + return -1; +} + + +void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + // row 0: the lower bound + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); + + // row 1: the upper bound + setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); + + for (int row=0;row<getNumRows();row++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + constraintRow.m_multiBodyA = m_bodyA; + constraintRow.m_multiBodyB = m_bodyB; + + btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse); + { + btScalar penetration = getPosition(row); + btScalar positionalError = 0.f; + btScalar velocityError = - rel_vel;// * damping; + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = penetrationImpulse; + } + } + } + +} + + + + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h new file mode 100644 index 00000000000..0c7fc170822 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -0,0 +1,44 @@ +/* +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_JOINT_LIMIT_CONSTRAINT_H +#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint +{ +protected: + + btScalar m_lowerBound; + btScalar m_upperBound; +public: + + btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); + virtual ~btMultiBodyJointLimitConstraint(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + +}; + +#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp new file mode 100644 index 00000000000..ab5a430231b --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -0,0 +1,89 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyJointMotor.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) + :btMultiBodyConstraint(body,body,link,link,1,true), + m_desiredVelocity(desiredVelocity) +{ + m_maxAppliedImpulse = maxMotorImpulse; + // the data.m_jacobians never change, so may as well + // initialize them here + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + + // row 0: the lower bound + jacobianA(0)[6 + link] = 1; +} +btMultiBodyJointMotor::~btMultiBodyJointMotor() +{ +} + +int btMultiBodyJointMotor::getIslandIdA() const +{ + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;i<m_bodyA->getNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + return -1; +} + +int btMultiBodyJointMotor::getIslandIdB() const +{ + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;i<m_bodyB->getNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + return -1; +} + + +void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + + + for (int row=0;row<getNumRows();row++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + + btScalar penetration = 0; + fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse); + } + +} + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h new file mode 100644 index 00000000000..43869348141 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_JOINT_MOTOR_H +#define BT_MULTIBODY_JOINT_MOTOR_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodyJointMotor : public btMultiBodyConstraint +{ +protected: + + + btScalar m_desiredVelocity; + +public: + + btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse); + virtual ~btMultiBodyJointMotor(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + +}; + +#endif //BT_MULTIBODY_JOINT_MOTOR_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h new file mode 100644 index 00000000000..fbd54c45db3 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -0,0 +1,110 @@ +/* +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" + +enum btMultiBodyLinkFlags +{ + BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 +}; +// +// 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; + + class btMultiBodyLinkCollider* m_collider; + int m_flags; + + // 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), + m_collider(0), + m_flags(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 diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h new file mode 100644 index 00000000000..27f12514a6d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -0,0 +1,92 @@ +/* +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) + { + m_checkCollideWith = true; + //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands + //this means that some constraints might point to bodies that are not in the islands, causing crashes + //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; + } + + virtual bool checkCollideWithOverride(const btCollisionObject* co) const + { + const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); + if (!other) + return true; + if (other->m_multiBody != this->m_multiBody) + return true; + if (!m_multiBody->hasSelfCollision()) + return false; + + //check if 'link' has collision disabled + if (m_link>=0) + { + const btMultibodyLink& link = m_multiBody->getLink(this->m_link); + if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link) + return false; + } + + if (other->m_link>=0) + { + const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); + if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link) + return false; + } + return true; + } +}; + +#endif //BT_FEATHERSTONE_LINK_COLLIDER_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp new file mode 100644 index 00000000000..f6690049156 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -0,0 +1,143 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyPoint2Point.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) + :btMultiBodyConstraint(body,0,link,-1,3,false), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) +{ +} + +btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) +{ +} + + +btMultiBodyPoint2Point::~btMultiBodyPoint2Point() +{ +} + + +int btMultiBodyPoint2Point::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;i<m_bodyA->getNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyPoint2Point::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;i<m_bodyB->getNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + + + +void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + +// int i=1; + for (int i=0;i<3;i++) + { + + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + + constraintRow.m_solverBodyIdA = data.m_fixedBodyId; + constraintRow.m_solverBodyIdB = data.m_fixedBodyId; + + + btVector3 contactNormalOnB(0,0,0); + contactNormalOnB[i] = -1; + + btScalar penetration = 0; + + // Convert local points back to world + btVector3 pivotAworld = m_pivotInA; + if (m_rigidBodyA) + { + + constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); + pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; + } else + { + if (m_bodyA) + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + } + btVector3 pivotBworld = m_pivotInB; + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; + } else + { + if (m_bodyB) + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + + } + btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB); + btScalar relaxation = 1.f; + fillMultiBodyConstraintMixed(constraintRow, data, + contactNormalOnB, + pivotAworld, pivotBworld, + position, + infoGlobal, + relaxation, + false); + constraintRow.m_lowerLimit = -m_maxAppliedImpulse; + constraintRow.m_upperLimit = m_maxAppliedImpulse; + + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h new file mode 100644 index 00000000000..26ca12b406d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -0,0 +1,60 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_POINT2POINT_H +#define BT_MULTIBODY_POINT2POINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyPoint2Point : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + + +public: + + btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); + btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); + + virtual ~btMultiBodyPoint2Point(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + +}; + +#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h new file mode 100644 index 00000000000..cf06dfb9ebf --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -0,0 +1,82 @@ +/* +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; +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint +{ + 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 |