mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-07-10 14:14:33 +00:00
Updated SDL, Bullet and OpenAL soft libs
Fixed case sensitivity problem Fixed clang compiler problem with having the class namespace used in an inline for the == operator Tweaked some theme stuff to be more consistent. Added initial test of no-pie for linux test sidestep of getTexCoord in shadergen hlsl feature so we don't assert when getting the terrain's shaderstuffs(which uses float3 instead of normal float2)
This commit is contained in:
Areloch
parent
e87dc787ee
commit
f8750dd8ed
1102 changed files with 205083 additions and 62836 deletions
|
|
@ -8,69 +8,84 @@
|
|||
#include "details/MultiBodyTreeImpl.hpp"
|
||||
#include "details/MultiBodyTreeInitCache.hpp"
|
||||
|
||||
namespace btInverseDynamics {
|
||||
|
||||
namespace btInverseDynamics
|
||||
{
|
||||
MultiBodyTree::MultiBodyTree()
|
||||
: m_is_finalized(false),
|
||||
m_mass_parameters_are_valid(true),
|
||||
m_accept_invalid_mass_parameters(false),
|
||||
m_impl(0x0),
|
||||
m_init_cache(0x0) {
|
||||
m_init_cache(0x0)
|
||||
{
|
||||
m_init_cache = new InitCache();
|
||||
}
|
||||
|
||||
MultiBodyTree::~MultiBodyTree() {
|
||||
MultiBodyTree::~MultiBodyTree()
|
||||
{
|
||||
delete m_impl;
|
||||
delete m_init_cache;
|
||||
}
|
||||
|
||||
void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) {
|
||||
void MultiBodyTree::setAcceptInvalidMassParameters(bool flag)
|
||||
{
|
||||
m_accept_invalid_mass_parameters = flag;
|
||||
}
|
||||
|
||||
bool MultiBodyTree::getAcceptInvalidMassProperties() const {
|
||||
bool MultiBodyTree::getAcceptInvalidMassProperties() const
|
||||
{
|
||||
return m_accept_invalid_mass_parameters;
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const {
|
||||
int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const
|
||||
{
|
||||
return m_impl->getBodyOrigin(body_index, world_origin);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const {
|
||||
int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const
|
||||
{
|
||||
return m_impl->getBodyCoM(body_index, world_com);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const {
|
||||
int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const
|
||||
{
|
||||
return m_impl->getBodyTransform(body_index, world_T_body);
|
||||
}
|
||||
int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const {
|
||||
int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const
|
||||
{
|
||||
return m_impl->getBodyAngularVelocity(body_index, world_omega);
|
||||
}
|
||||
int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const {
|
||||
int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const
|
||||
{
|
||||
return m_impl->getBodyLinearVelocity(body_index, world_velocity);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const {
|
||||
int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const
|
||||
{
|
||||
return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const {
|
||||
int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const
|
||||
{
|
||||
return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
|
||||
}
|
||||
int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const {
|
||||
int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const
|
||||
{
|
||||
return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const {
|
||||
return m_impl->getParentRParentBodyRef(body_index, r);
|
||||
int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3 *r) const
|
||||
{
|
||||
return m_impl->getParentRParentBodyRef(body_index, r);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const {
|
||||
return m_impl->getBodyTParentRef(body_index, T);
|
||||
int MultiBodyTree::getBodyTParentRef(const int body_index, mat33 *T) const
|
||||
{
|
||||
return m_impl->getBodyTParentRef(body_index, T);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const {
|
||||
return m_impl->getBodyAxisOfMotion(body_index, axis);
|
||||
int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
|
||||
{
|
||||
return m_impl->getBodyAxisOfMotion(body_index, axis);
|
||||
}
|
||||
|
||||
void MultiBodyTree::printTree() { m_impl->printTree(); }
|
||||
|
|
@ -81,13 +96,16 @@ int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
|
|||
int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
|
||||
|
||||
int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
|
||||
vecx *joint_forces) {
|
||||
if (false == m_is_finalized) {
|
||||
error_message("system has not been initialized\n");
|
||||
vecx *joint_forces)
|
||||
{
|
||||
if (false == m_is_finalized)
|
||||
{
|
||||
bt_id_error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
|
||||
error_message("error in inverse dynamics calculation\n");
|
||||
if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces))
|
||||
{
|
||||
bt_id_error_message("error in inverse dynamics calculation\n");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
|
|
@ -95,142 +113,165 @@ int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const
|
|||
|
||||
int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
|
||||
const bool initialize_matrix,
|
||||
const bool set_lower_triangular_matrix, matxx *mass_matrix) {
|
||||
if (false == m_is_finalized) {
|
||||
error_message("system has not been initialized\n");
|
||||
const bool set_lower_triangular_matrix, matxx *mass_matrix)
|
||||
{
|
||||
if (false == m_is_finalized)
|
||||
{
|
||||
bt_id_error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 ==
|
||||
m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
|
||||
set_lower_triangular_matrix, mass_matrix)) {
|
||||
error_message("error in mass matrix calculation\n");
|
||||
set_lower_triangular_matrix, mass_matrix))
|
||||
{
|
||||
bt_id_error_message("error in mass matrix calculation\n");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) {
|
||||
int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix)
|
||||
{
|
||||
return calculateMassMatrix(q, true, true, true, mass_matrix);
|
||||
}
|
||||
|
||||
int MultiBodyTree::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u)
|
||||
{
|
||||
vec3 world_gravity(m_impl->m_world_gravity);
|
||||
// temporarily set gravity to zero, to ensure we get the actual accelerations
|
||||
setZero(m_impl->m_world_gravity);
|
||||
|
||||
if (false == m_is_finalized)
|
||||
{
|
||||
bt_id_error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateKinematics(q, u, dot_u,
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION))
|
||||
{
|
||||
bt_id_error_message("error in kinematics calculation\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) {
|
||||
vec3 world_gravity(m_impl->m_world_gravity);
|
||||
// temporarily set gravity to zero, to ensure we get the actual accelerations
|
||||
setZero(m_impl->m_world_gravity);
|
||||
|
||||
if (false == m_is_finalized) {
|
||||
error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateKinematics(q, u, dot_u,
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) {
|
||||
error_message("error in kinematics calculation\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
m_impl->m_world_gravity=world_gravity;
|
||||
return 0;
|
||||
m_impl->m_world_gravity = world_gravity;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int MultiBodyTree::calculatePositionKinematics(const vecx& q) {
|
||||
if (false == m_is_finalized) {
|
||||
error_message("system has not been initialized\n");
|
||||
int MultiBodyTree::calculatePositionKinematics(const vecx &q)
|
||||
{
|
||||
if (false == m_is_finalized)
|
||||
{
|
||||
bt_id_error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateKinematics(q, q, q,
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
|
||||
error_message("error in kinematics calculation\n");
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
|
||||
{
|
||||
bt_id_error_message("error in kinematics calculation\n");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) {
|
||||
if (false == m_is_finalized) {
|
||||
error_message("system has not been initialized\n");
|
||||
int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx &q, const vecx &u)
|
||||
{
|
||||
if (false == m_is_finalized)
|
||||
{
|
||||
bt_id_error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateKinematics(q, u, u,
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
|
||||
error_message("error in kinematics calculation\n");
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
|
||||
{
|
||||
bt_id_error_message("error in kinematics calculation\n");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
|
||||
int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) {
|
||||
if (false == m_is_finalized) {
|
||||
error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateJacobians(q, u,
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) {
|
||||
error_message("error in jacobian calculation\n");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
int MultiBodyTree::calculateJacobians(const vecx &q, const vecx &u)
|
||||
{
|
||||
if (false == m_is_finalized)
|
||||
{
|
||||
bt_id_error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateJacobians(q, u,
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
|
||||
{
|
||||
bt_id_error_message("error in jacobian calculation\n");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MultiBodyTree::calculateJacobians(const vecx& q){
|
||||
if (false == m_is_finalized) {
|
||||
error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateJacobians(q, q,
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) {
|
||||
error_message("error in jacobian calculation\n");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
int MultiBodyTree::calculateJacobians(const vecx &q)
|
||||
{
|
||||
if (false == m_is_finalized)
|
||||
{
|
||||
bt_id_error_message("system has not been initialized\n");
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->calculateJacobians(q, q,
|
||||
MultiBodyTree::MultiBodyImpl::POSITION_ONLY))
|
||||
{
|
||||
bt_id_error_message("error in jacobian calculation\n");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
|
||||
return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u);
|
||||
int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
|
||||
{
|
||||
return m_impl->getBodyDotJacobianTransU(body_index, world_dot_jac_trans_u);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const {
|
||||
return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u);
|
||||
int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
|
||||
{
|
||||
return m_impl->getBodyDotJacobianRotU(body_index, world_dot_jac_rot_u);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const {
|
||||
return m_impl->getBodyJacobianTrans(body_index,world_jac_trans);
|
||||
int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
|
||||
{
|
||||
return m_impl->getBodyJacobianTrans(body_index, world_jac_trans);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const {
|
||||
return m_impl->getBodyJacobianRot(body_index,world_jac_rot);
|
||||
int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
|
||||
{
|
||||
return m_impl->getBodyJacobianRot(body_index, world_jac_rot);
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
|
||||
const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
|
||||
const vec3 &body_axis_of_motion_, idScalar mass,
|
||||
const vec3 &body_r_body_com, const mat33 &body_I_body,
|
||||
const int user_int, void *user_ptr) {
|
||||
if (body_index < 0) {
|
||||
error_message("body index must be positive (got %d)\n", body_index);
|
||||
const int user_int, void *user_ptr)
|
||||
{
|
||||
if (body_index < 0)
|
||||
{
|
||||
bt_id_error_message("body index must be positive (got %d)\n", body_index);
|
||||
return -1;
|
||||
}
|
||||
vec3 body_axis_of_motion(body_axis_of_motion_);
|
||||
switch (joint_type) {
|
||||
switch (joint_type)
|
||||
{
|
||||
case REVOLUTE:
|
||||
case PRISMATIC:
|
||||
// check if axis is unit vector
|
||||
if (!isUnitVector(body_axis_of_motion)) {
|
||||
warning_message(
|
||||
if (!isUnitVector(body_axis_of_motion))
|
||||
{
|
||||
bt_id_warning_message(
|
||||
"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
|
||||
body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
|
||||
idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
|
||||
BT_ID_POW(body_axis_of_motion(1), 2) +
|
||||
BT_ID_POW(body_axis_of_motion(2), 2));
|
||||
if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min())) {
|
||||
error_message("axis of motion vector too short (%e)\n", length);
|
||||
BT_ID_POW(body_axis_of_motion(1), 2) +
|
||||
BT_ID_POW(body_axis_of_motion(2), 2));
|
||||
if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min()))
|
||||
{
|
||||
bt_id_error_message("axis of motion vector too short (%e)\n", length);
|
||||
return -1;
|
||||
}
|
||||
body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
|
||||
|
|
@ -240,29 +281,36 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
|
|||
break;
|
||||
case FLOATING:
|
||||
break;
|
||||
case SPHERICAL:
|
||||
break;
|
||||
default:
|
||||
error_message("unknown joint type %d\n", joint_type);
|
||||
bt_id_error_message("unknown joint type %d\n", joint_type);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// sanity check for mass properties. Zero mass is OK.
|
||||
if (mass < 0) {
|
||||
if (mass < 0)
|
||||
{
|
||||
m_mass_parameters_are_valid = false;
|
||||
error_message("Body %d has invalid mass %e\n", body_index, mass);
|
||||
if (!m_accept_invalid_mass_parameters) {
|
||||
bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass);
|
||||
if (!m_accept_invalid_mass_parameters)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) {
|
||||
if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type))
|
||||
{
|
||||
m_mass_parameters_are_valid = false;
|
||||
// error message printed in function call
|
||||
if (!m_accept_invalid_mass_parameters) {
|
||||
if (!m_accept_invalid_mass_parameters)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (!isValidTransformMatrix(body_T_parent_ref)) {
|
||||
if (!isValidTransformMatrix(body_T_parent_ref))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -271,52 +319,63 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
|
|||
body_I_body, user_int, user_ptr);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const {
|
||||
int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const
|
||||
{
|
||||
return m_impl->getParentIndex(body_index, parent_index);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getUserInt(const int body_index, int *user_int) const {
|
||||
int MultiBodyTree::getUserInt(const int body_index, int *user_int) const
|
||||
{
|
||||
return m_impl->getUserInt(body_index, user_int);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const {
|
||||
int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const
|
||||
{
|
||||
return m_impl->getUserPtr(body_index, user_ptr);
|
||||
}
|
||||
|
||||
int MultiBodyTree::setUserInt(const int body_index, const int user_int) {
|
||||
int MultiBodyTree::setUserInt(const int body_index, const int user_int)
|
||||
{
|
||||
return m_impl->setUserInt(body_index, user_int);
|
||||
}
|
||||
|
||||
int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) {
|
||||
int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr)
|
||||
{
|
||||
return m_impl->setUserPtr(body_index, user_ptr);
|
||||
}
|
||||
|
||||
int MultiBodyTree::finalize() {
|
||||
int MultiBodyTree::finalize()
|
||||
{
|
||||
const int &num_bodies = m_init_cache->numBodies();
|
||||
const int &num_dofs = m_init_cache->numDoFs();
|
||||
|
||||
if(num_dofs<=0) {
|
||||
error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
|
||||
//return -1;
|
||||
}
|
||||
if (num_dofs < 0)
|
||||
{
|
||||
bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
|
||||
//return -1;
|
||||
}
|
||||
|
||||
// 1 allocate internal MultiBody structure
|
||||
m_impl = new MultiBodyImpl(num_bodies, num_dofs);
|
||||
|
||||
// 2 build new index set assuring index(parent) < index(child)
|
||||
if (-1 == m_init_cache->buildIndexSets()) {
|
||||
if (-1 == m_init_cache->buildIndexSets())
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
|
||||
|
||||
// 3 setup internal kinematic and dynamic data
|
||||
for (int index = 0; index < num_bodies; index++) {
|
||||
for (int index = 0; index < num_bodies; index++)
|
||||
{
|
||||
InertiaData inertia;
|
||||
JointData joint;
|
||||
if (-1 == m_init_cache->getInertiaData(index, &inertia)) {
|
||||
if (-1 == m_init_cache->getInertiaData(index, &inertia))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_init_cache->getJointData(index, &joint)) {
|
||||
if (-1 == m_init_cache->getJointData(index, &joint))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -331,9 +390,30 @@ int MultiBodyTree::finalize() {
|
|||
rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
|
||||
rigid_body.m_joint_type = joint.m_type;
|
||||
|
||||
int user_int;
|
||||
if (-1 == m_init_cache->getUserInt(index, &user_int))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->setUserInt(index, user_int))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
void *user_ptr;
|
||||
if (-1 == m_init_cache->getUserPtr(index, &user_ptr))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
if (-1 == m_impl->setUserPtr(index, user_ptr))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
|
||||
// matrices.
|
||||
switch (rigid_body.m_joint_type) {
|
||||
switch (rigid_body.m_joint_type)
|
||||
{
|
||||
case REVOLUTE:
|
||||
rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
|
||||
rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1);
|
||||
|
|
@ -359,6 +439,16 @@ int MultiBodyTree::finalize() {
|
|||
rigid_body.m_Jac_JT(1) = 0.0;
|
||||
rigid_body.m_Jac_JT(2) = 0.0;
|
||||
break;
|
||||
case SPHERICAL:
|
||||
// NOTE/TODO: this is not really correct.
|
||||
// the Jacobians should be 3x3 matrices here !
|
||||
rigid_body.m_Jac_JR(0) = 0.0;
|
||||
rigid_body.m_Jac_JR(1) = 0.0;
|
||||
rigid_body.m_Jac_JR(2) = 0.0;
|
||||
rigid_body.m_Jac_JT(0) = 0.0;
|
||||
rigid_body.m_Jac_JT(1) = 0.0;
|
||||
rigid_body.m_Jac_JT(2) = 0.0;
|
||||
break;
|
||||
case FLOATING:
|
||||
// NOTE/TODO: this is not really correct.
|
||||
// the Jacobians should be 3x3 matrices here !
|
||||
|
|
@ -370,14 +460,15 @@ int MultiBodyTree::finalize() {
|
|||
rigid_body.m_Jac_JT(2) = 0.0;
|
||||
break;
|
||||
default:
|
||||
error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
|
||||
bt_id_error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// 4 assign degree of freedom indices & build per-joint-type index arrays
|
||||
if (-1 == m_impl->generateIndexSets()) {
|
||||
error_message("generating index sets\n");
|
||||
if (-1 == m_impl->generateIndexSets())
|
||||
{
|
||||
bt_id_error_message("generating index sets\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -392,54 +483,66 @@ int MultiBodyTree::finalize() {
|
|||
return 0;
|
||||
}
|
||||
|
||||
int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) {
|
||||
int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity)
|
||||
{
|
||||
return m_impl->setGravityInWorldFrame(gravity);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const {
|
||||
int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const
|
||||
{
|
||||
return m_impl->getJointType(body_index, joint_type);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const {
|
||||
int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const
|
||||
{
|
||||
return m_impl->getJointTypeStr(body_index, joint_type);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const {
|
||||
int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const
|
||||
{
|
||||
return m_impl->getDoFOffset(body_index, q_offset);
|
||||
}
|
||||
|
||||
int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) {
|
||||
int MultiBodyTree::setBodyMass(const int body_index, idScalar mass)
|
||||
{
|
||||
return m_impl->setBodyMass(body_index, mass);
|
||||
}
|
||||
|
||||
int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) {
|
||||
int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3 &first_mass_moment)
|
||||
{
|
||||
return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
|
||||
}
|
||||
|
||||
int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) {
|
||||
int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33 &second_mass_moment)
|
||||
{
|
||||
return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const {
|
||||
int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const
|
||||
{
|
||||
return m_impl->getBodyMass(body_index, mass);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const {
|
||||
int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const
|
||||
{
|
||||
return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
|
||||
}
|
||||
|
||||
int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const {
|
||||
int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const
|
||||
{
|
||||
return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
|
||||
}
|
||||
|
||||
void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
|
||||
|
||||
int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) {
|
||||
int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force)
|
||||
{
|
||||
return m_impl->addUserForce(body_index, body_force);
|
||||
}
|
||||
|
||||
int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) {
|
||||
int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment)
|
||||
{
|
||||
return m_impl->addUserMoment(body_index, body_moment);
|
||||
}
|
||||
|
||||
}
|
||||
} // namespace btInverseDynamics
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue