mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-07 08:10:08 +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!
82 lines
2.4 KiB
C++
82 lines
2.4 KiB
C++
#include "RandomTreeCreator.hpp"
|
|
|
|
#include <cstdio>
|
|
|
|
#include "IDRandomUtil.hpp"
|
|
|
|
namespace btInverseDynamics {
|
|
|
|
RandomTreeCreator::RandomTreeCreator(const int max_bodies, bool random_seed) {
|
|
// seed generator
|
|
if(random_seed) {
|
|
randomInit(); // seeds with time()
|
|
} else {
|
|
randomInit(1); // seeds with 1
|
|
}
|
|
m_num_bodies = randomInt(1, max_bodies);
|
|
}
|
|
|
|
RandomTreeCreator::~RandomTreeCreator() {}
|
|
|
|
int RandomTreeCreator::getNumBodies(int* num_bodies) const {
|
|
*num_bodies = m_num_bodies;
|
|
return 0;
|
|
}
|
|
|
|
int RandomTreeCreator::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 {
|
|
if(0 == body_index) { //root body
|
|
*parent_index = -1;
|
|
} else {
|
|
*parent_index = randomInt(0, body_index - 1);
|
|
}
|
|
|
|
switch (randomInt(0, 3)) {
|
|
case 0:
|
|
*joint_type = FIXED;
|
|
break;
|
|
case 1:
|
|
*joint_type = REVOLUTE;
|
|
break;
|
|
case 2:
|
|
*joint_type = PRISMATIC;
|
|
break;
|
|
case 3:
|
|
*joint_type = FLOATING;
|
|
break;
|
|
default:
|
|
error_message("randomInt() result out of range\n");
|
|
return -1;
|
|
}
|
|
|
|
(*parent_r_parent_body_ref)(0) = randomFloat(-1.0, 1.0);
|
|
(*parent_r_parent_body_ref)(1) = randomFloat(-1.0, 1.0);
|
|
(*parent_r_parent_body_ref)(2) = randomFloat(-1.0, 1.0);
|
|
|
|
bodyTParentFromAxisAngle(randomAxis(), randomFloat(-BT_ID_PI, BT_ID_PI), body_T_parent_ref);
|
|
|
|
*body_axis_of_motion = randomAxis();
|
|
*mass = randomMass();
|
|
(*body_r_body_com)(0) = randomFloat(-1.0, 1.0);
|
|
(*body_r_body_com)(1) = randomFloat(-1.0, 1.0);
|
|
(*body_r_body_com)(2) = randomFloat(-1.0, 1.0);
|
|
const double a = randomFloat(-BT_ID_PI, BT_ID_PI);
|
|
const double b = randomFloat(-BT_ID_PI, BT_ID_PI);
|
|
const double c = randomFloat(-BT_ID_PI, BT_ID_PI);
|
|
vec3 ii = randomInertiaPrincipal();
|
|
mat33 ii_diag;
|
|
setZero(ii_diag);
|
|
ii_diag(0,0)=ii(0);
|
|
ii_diag(1,1)=ii(1);
|
|
ii_diag(2,2)=ii(2);
|
|
*body_I_body = transformX(a) * transformY(b) * transformZ(c) * ii_diag *
|
|
transformZ(-c) * transformY(-b) * transformX(-a);
|
|
*user_int = 0;
|
|
*user_ptr = 0;
|
|
|
|
return 0;
|
|
}
|
|
}
|