mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 14:10:11 +00:00
ba8964c4ac
This change adds support for calculating Jacobians and dot(Jacobian)*u terms, along with the required support for the 3xN matrices in the standalone math library. It also adds functions to compute kinematics only (position, velocity, accel). To facilitate tests, the Cl also adds a RandomTreeCreator to create randomized multibody trees. Thanks to Thomas Buschmann for this contribution!
50 lines
2.6 KiB
C++
50 lines
2.6 KiB
C++
#include "CloneTreeCreator.hpp"
|
|
|
|
#include <cstdio>
|
|
|
|
namespace btInverseDynamics {
|
|
#define CHECK_NULLPTR() \
|
|
do { \
|
|
if (m_reference == 0x0) { \
|
|
error_message("m_reference == 0x0\n"); \
|
|
return -1; \
|
|
} \
|
|
} while (0)
|
|
|
|
#define TRY(x) \
|
|
do { \
|
|
if (x == -1) { \
|
|
error_message("error calling " #x "\n"); \
|
|
return -1; \
|
|
} \
|
|
} while (0)
|
|
CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference) { m_reference = reference; }
|
|
|
|
CloneTreeCreator::~CloneTreeCreator() {}
|
|
|
|
int CloneTreeCreator::getNumBodies(int* num_bodies) const {
|
|
CHECK_NULLPTR();
|
|
*num_bodies = m_reference->numBodies();
|
|
return 0;
|
|
}
|
|
|
|
int CloneTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
|
|
vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
|
|
vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
|
|
mat33* body_I_body, int* user_int, void** user_ptr) const {
|
|
CHECK_NULLPTR();
|
|
TRY(m_reference->getParentIndex(body_index, parent_index));
|
|
TRY(m_reference->getJointType(body_index, joint_type));
|
|
TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref));
|
|
TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref));
|
|
TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion));
|
|
TRY(m_reference->getBodyMass(body_index, mass));
|
|
TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com));
|
|
TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body));
|
|
TRY(m_reference->getUserInt(body_index, user_int));
|
|
TRY(m_reference->getUserPtr(body_index, user_ptr));
|
|
|
|
return 0;
|
|
}
|
|
}
|