static void
print_memdevmap(smbios_hdl_t *shp, id_t id, FILE *fp)
{
smbios_memdevmap_t md;
(void) smbios_info_memdevmap(shp, id, &md);
id_printf(fp, " Memory Device: ", md.smbmdm_device);
id_printf(fp, " Memory Array Mapped Address: ", md.smbmdm_arrmap);
oprintf(fp, " Physical Address: 0x%llx\n Size: %llu bytes\n",
(u_longlong_t)md.smbmdm_addr, (u_longlong_t)md.smbmdm_size);
oprintf(fp, " Partition Row Position: %u\n", md.smbmdm_rpos);
oprintf(fp, " Interleave Position: %u\n", md.smbmdm_ipos);
oprintf(fp, " Interleave Data Depth: %u\n", md.smbmdm_idepth);
}
static void
print_memarrmap(smbios_hdl_t *shp, id_t id, FILE *fp)
{
smbios_memarrmap_t ma;
(void) smbios_info_memarrmap(shp, id, &ma);
id_printf(fp, " Physical Memory Array: ", ma.smbmam_array);
oprintf(fp, " Devices per Row: %u\n", ma.smbmam_width);
oprintf(fp, " Physical Address: 0x%llx\n Size: %llu bytes\n",
(u_longlong_t)ma.smbmam_addr, (u_longlong_t)ma.smbmam_size);
}
static void
print_memarray(smbios_hdl_t *shp, id_t id, FILE *fp)
{
smbios_memarray_t ma;
(void) smbios_info_memarray(shp, id, &ma);
desc_printf(smbios_memarray_loc_desc(ma.smbma_location),
fp, " Location: %u", ma.smbma_location);
desc_printf(smbios_memarray_use_desc(ma.smbma_use),
fp, " Use: %u", ma.smbma_use);
desc_printf(smbios_memarray_ecc_desc(ma.smbma_ecc),
fp, " ECC: %u", ma.smbma_ecc);
oprintf(fp, " Number of Slots/Sockets: %u\n", ma.smbma_ndevs);
id_printf(fp, " Memory Error Data: ", ma.smbma_err);
oprintf(fp, " Max Capacity: %llu bytes\n",
(u_longlong_t)ma.smbma_size);
}
int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose) {
if (0x0 == btmb) {
error_message("cannot create MultiBodyTree from null pointer\n");
return -1;
}
// in case this is a second call, discard old data
m_data.clear();
m_initialized = false;
// btMultiBody treats base link separately
m_data.resize(1 + btmb->getNumLinks());
// add base link data
{
LinkData &link = m_data[0];
link.parent_index = -1;
if (btmb->hasFixedBase()) {
link.joint_type = FIXED;
} else {
link.joint_type = FLOATING;
}
btTransform transform=(btmb->getBaseWorldTransform());
//compute inverse dynamics in body-fixed frame
transform.setIdentity();
link.parent_r_parent_body_ref(0) = transform.getOrigin()[0];
link.parent_r_parent_body_ref(1) = transform.getOrigin()[1];
link.parent_r_parent_body_ref(2) = transform.getOrigin()[2];
link.body_T_parent_ref(0, 0) = transform.getBasis()[0][0];
link.body_T_parent_ref(0, 1) = transform.getBasis()[0][1];
link.body_T_parent_ref(0, 2) = transform.getBasis()[0][2];
link.body_T_parent_ref(1, 0) = transform.getBasis()[1][0];
link.body_T_parent_ref(1, 1) = transform.getBasis()[1][1];
link.body_T_parent_ref(1, 2) = transform.getBasis()[1][2];
link.body_T_parent_ref(2, 0) = transform.getBasis()[2][0];
link.body_T_parent_ref(2, 1) = transform.getBasis()[2][1];
link.body_T_parent_ref(2, 2) = transform.getBasis()[2][2];
// random unit vector. value not used for fixed or floating joints.
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
link.mass = btmb->getBaseMass();
// link frame in the center of mass
link.body_r_body_com(0) = 0;
link.body_r_body_com(1) = 0;
link.body_r_body_com(2) = 0;
// BulletDynamics uses body-fixed frame in the cog, aligned with principal axes
link.body_I_body(0, 0) = btmb->getBaseInertia()[0];
link.body_I_body(0, 1) = 0.0;
link.body_I_body(0, 2) = 0.0;
link.body_I_body(1, 0) = 0.0;
link.body_I_body(1, 1) = btmb->getBaseInertia()[1];
link.body_I_body(1, 2) = 0.0;
link.body_I_body(2, 0) = 0.0;
link.body_I_body(2, 1) = 0.0;
link.body_I_body(2, 2) = btmb->getBaseInertia()[2];
// shift reference point to link origin (in joint axis)
mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
if (verbose) {
id_printf("base: mass= %f, bt_inertia= [%f %f %f]\n"
"Io= [%f %f %f;\n"
" %f %f %f;\n"
" %f %f %f]\n",
link.mass, btmb->getBaseInertia()[0], btmb->getBaseInertia()[1],
btmb->getBaseInertia()[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
link.body_I_body(2, 2));
}
}
for (int bt_index = 0; bt_index < btmb->getNumLinks(); bt_index++) {
if (verbose) {
id_printf("bt->id: converting link %d\n", bt_index);
}
const btMultibodyLink &bt_link = btmb->getLink(bt_index);
LinkData &link = m_data[bt_index + 1];
link.parent_index = bt_link.m_parent + 1;
link.mass = bt_link.m_mass;
if (verbose) {
id_printf("mass= %f\n", link.mass);
}
// from this body's pivot to this body's com in this body's frame
link.body_r_body_com[0] = bt_link.m_dVector[0];
link.body_r_body_com[1] = bt_link.m_dVector[1];
link.body_r_body_com[2] = bt_link.m_dVector[2];
if (verbose) {
id_printf("com= %f %f %f\n", link.body_r_body_com[0], link.body_r_body_com[1],
link.body_r_body_com[2]);
}
// BulletDynamics uses a body-fixed frame in the CoM, aligned with principal axes
link.body_I_body(0, 0) = bt_link.m_inertiaLocal[0];
link.body_I_body(0, 1) = 0.0;
link.body_I_body(0, 2) = 0.0;
link.body_I_body(1, 0) = 0.0;
link.body_I_body(1, 1) = bt_link.m_inertiaLocal[1];
link.body_I_body(1, 2) = 0.0;
link.body_I_body(2, 0) = 0.0;
link.body_I_body(2, 1) = 0.0;
link.body_I_body(2, 2) = bt_link.m_inertiaLocal[2];
// shift reference point to link origin (in joint axis)
mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
if (verbose) {
id_printf("link %d: mass= %f, bt_inertia= [%f %f %f]\n"
"Io= [%f %f %f;\n"
" %f %f %f;\n"
" %f %f %f]\n",
bt_index, link.mass, bt_link.m_inertiaLocal[0], bt_link.m_inertiaLocal[1],
bt_link.m_inertiaLocal[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
link.body_I_body(2, 2));
}
// transform for vectors written in parent frame to this link's body-fixed frame
btMatrix3x3 basis = btTransform(bt_link.m_zeroRotParentToThis).getBasis();
link.body_T_parent_ref(0, 0) = basis[0][0];
link.body_T_parent_ref(0, 1) = basis[0][1];
link.body_T_parent_ref(0, 2) = basis[0][2];
link.body_T_parent_ref(1, 0) = basis[1][0];
link.body_T_parent_ref(1, 1) = basis[1][1];
link.body_T_parent_ref(1, 2) = basis[1][2];
link.body_T_parent_ref(2, 0) = basis[2][0];
link.body_T_parent_ref(2, 1) = basis[2][1];
link.body_T_parent_ref(2, 2) = basis[2][2];
if (verbose) {
id_printf("body_T_parent_ref= %f %f %f\n"
" %f %f %f\n"
" %f %f %f\n",
basis[0][0], basis[0][1], basis[0][2], basis[1][0], basis[1][1], basis[1][2],
basis[2][0], basis[2][1], basis[2][2]);
}
switch (bt_link.m_jointType) {
case btMultibodyLink::eRevolute:
link.joint_type = REVOLUTE;
if (verbose) {
id_printf("type= revolute\n");
}
link.body_axis_of_motion(0) = bt_link.m_axes[0].m_topVec[0];
link.body_axis_of_motion(1) = bt_link.m_axes[0].m_topVec[1];
link.body_axis_of_motion(2) = bt_link.m_axes[0].m_topVec[2];
// for revolute joints, m_eVector = parentComToThisPivotOffset
// m_dVector = thisPivotToThisComOffset
// from parent com to pivot, in parent frame
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::ePrismatic:
link.joint_type = PRISMATIC;
if (verbose) {
id_printf("type= prismatic\n");
}
link.body_axis_of_motion(0) = bt_link.m_axes[0].m_bottomVec[0];
link.body_axis_of_motion(1) = bt_link.m_axes[0].m_bottomVec[1];
link.body_axis_of_motion(2) = bt_link.m_axes[0].m_bottomVec[2];
// for prismatic joints, eVector
// according to documentation :
// parentComToThisComOffset
// but seems to be: from parent's com to parent's
// pivot ??
// m_dVector = thisPivotToThisComOffset
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::eSpherical:
error_message("spherical joints not implemented\n");
return -1;
case btMultibodyLink::ePlanar:
error_message("planar joints not implemented\n");
return -1;
case btMultibodyLink::eFixed:
link.joint_type = FIXED;
// random unit vector
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
// for fixed joints, m_dVector = thisPivotToThisComOffset;
// m_eVector = parentComToThisPivotOffset;
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
default:
error_message("unknown btMultiBody::eFeatherstoneJointType %d\n",
bt_link.m_jointType);
return -1;
}
if (link.parent_index > 0) { // parent body isn't the root
const btMultibodyLink &bt_parent_link = btmb->getLink(link.parent_index - 1);
// from parent pivot to parent com, in parent frame
link.parent_r_parent_body_ref(0) += bt_parent_link.m_dVector[0];
link.parent_r_parent_body_ref(1) += bt_parent_link.m_dVector[1];
link.parent_r_parent_body_ref(2) += bt_parent_link.m_dVector[2];
} else {
// parent is root body. btMultiBody only knows 6-DoF or 0-DoF root bodies,
// whose link frame is in the CoM (ie, no notion of a pivot point)
}
if (verbose) {
id_printf("parent_r_parent_body_ref= %f %f %f\n", link.parent_r_parent_body_ref[0],
link.parent_r_parent_body_ref[1], link.parent_r_parent_body_ref[2]);
}
}
m_initialized = true;
return 0;
}
static void
print_memdevice(smbios_hdl_t *shp, id_t id, FILE *fp)
{
smbios_memdevice_t md;
(void) smbios_info_memdevice(shp, id, &md);
id_printf(fp, " Physical Memory Array: ", md.smbmd_array);
id_printf(fp, " Memory Error Data: ", md.smbmd_error);
if (md.smbmd_twidth != -1u)
oprintf(fp, " Total Width: %u bits\n", md.smbmd_twidth);
else
oprintf(fp, " Total Width: Unknown\n");
if (md.smbmd_dwidth != -1u)
oprintf(fp, " Data Width: %u bits\n", md.smbmd_dwidth);
else
oprintf(fp, " Data Width: Unknown\n");
switch (md.smbmd_size) {
case -1ull:
oprintf(fp, " Size: Unknown\n");
break;
case 0:
oprintf(fp, " Size: Not Populated\n");
break;
default:
oprintf(fp, " Size: %llu bytes\n",
(u_longlong_t)md.smbmd_size);
}
desc_printf(smbios_memdevice_form_desc(md.smbmd_form),
fp, " Form Factor: %u", md.smbmd_form);
if (md.smbmd_set == 0)
oprintf(fp, " Set: None\n");
else if (md.smbmd_set == (uint8_t)-1u)
oprintf(fp, " Set: Unknown\n");
else
oprintf(fp, " Set: %u\n", md.smbmd_set);
if (md.smbmd_rank != 0) {
desc_printf(smbios_memdevice_rank_desc(md.smbmd_rank),
fp, " Rank: %u", md.smbmd_rank);
} else {
oprintf(fp, " Rank: Unknown\n");
}
desc_printf(smbios_memdevice_type_desc(md.smbmd_type),
fp, " Memory Type: %u", md.smbmd_type);
flag_printf(fp, "Flags", md.smbmd_flags, sizeof (md.smbmd_flags) * NBBY,
smbios_memdevice_flag_name, smbios_memdevice_flag_desc);
if (md.smbmd_speed != 0)
oprintf(fp, " Speed: %u MHz\n", md.smbmd_speed);
else
oprintf(fp, " Speed: Unknown\n");
if (md.smbmd_clkspeed != 0)
oprintf(fp, " Configured Speed: %u MHz\n", md.smbmd_clkspeed);
else
oprintf(fp, " Configured Speed: Unknown\n");
oprintf(fp, " Device Locator: %s\n", md.smbmd_dloc);
oprintf(fp, " Bank Locator: %s\n", md.smbmd_bloc);
if (md.smbmd_minvolt != 0) {
oprintf(fp, " Minimum Voltage: %.2fV\n",
md.smbmd_minvolt / 1000.0);
} else {
oprintf(fp, " Minimum Voltage: Unknown\n");
}
if (md.smbmd_maxvolt != 0) {
oprintf(fp, " Maximum Voltage: %.2fV\n",
md.smbmd_maxvolt / 1000.0);
} else {
oprintf(fp, " Maximum Voltage: Unknown\n");
}
if (md.smbmd_confvolt != 0) {
oprintf(fp, " Configured Voltage: %.2fV\n",
md.smbmd_confvolt / 1000.0);
} else {
oprintf(fp, " Configured Voltage: Unknown\n");
}
}
static void
print_processor(smbios_hdl_t *shp, id_t id, FILE *fp)
{
smbios_processor_t p;
uint_t status;
(void) smbios_info_processor(shp, id, &p);
status = SMB_PRSTATUS_STATUS(p.smbp_status);
desc_printf(smbios_processor_family_desc(p.smbp_family),
fp, " Family: %u", p.smbp_family);
if (p.smbp_family2 != 0)
desc_printf(smbios_processor_family_desc(p.smbp_family2),
fp, " Family Ext: %u", p.smbp_family2);
oprintf(fp, " CPUID: 0x%llx\n", (u_longlong_t)p.smbp_cpuid);
desc_printf(smbios_processor_type_desc(p.smbp_type),
fp, " Type: %u", p.smbp_type);
desc_printf(smbios_processor_upgrade_desc(p.smbp_upgrade),
fp, " Socket Upgrade: %u", p.smbp_upgrade);
oprintf(fp, " Socket Status: %s\n",
SMB_PRSTATUS_PRESENT(p.smbp_status) ?
"Populated" : "Not Populated");
desc_printf(smbios_processor_status_desc(status),
fp, " Processor Status: %u", status);
if (SMB_PRV_LEGACY(p.smbp_voltage)) {
oprintf(fp, " Supported Voltages:");
switch (p.smbp_voltage) {
case SMB_PRV_5V:
oprintf(fp, " 5.0V");
break;
case SMB_PRV_33V:
oprintf(fp, " 3.3V");
break;
case SMB_PRV_29V:
oprintf(fp, " 2.9V");
break;
}
oprintf(fp, "\n");
} else {
oprintf(fp, " Supported Voltages: %.1fV\n",
(float)SMB_PRV_VOLTAGE(p.smbp_voltage) / 10);
}
if (p.smbp_corecount != 0) {
if (p.smbp_corecount != 0xff || p.smbp_corecount2 == 0)
oprintf(fp, " Core Count: %u\n", p.smbp_corecount);
else
oprintf(fp, " Core Count: %u\n", p.smbp_corecount2);
} else {
oprintf(fp, " Core Count: Unknown\n");
}
if (p.smbp_coresenabled != 0) {
if (p.smbp_coresenabled != 0xff || p.smbp_coresenabled2 == 0) {
oprintf(fp, " Cores Enabled: %u\n",
p.smbp_coresenabled);
} else {
oprintf(fp, " Cores Enabled: %u\n",
p.smbp_coresenabled2);
}
} else {
oprintf(fp, " Cores Enabled: Unknown\n");
}
if (p.smbp_threadcount != 0) {
if (p.smbp_threadcount != 0xff || p.smbp_threadcount2 == 0) {
oprintf(fp, " Thread Count: %u\n",
p.smbp_threadcount);
} else {
oprintf(fp, " Thread Count: %u\n",
p.smbp_threadcount2);
}
} else {
oprintf(fp, " Thread Count: Unknown\n");
}
if (p.smbp_cflags) {
flag_printf(fp, "Processor Characteristics",
p.smbp_cflags, sizeof (p.smbp_cflags) * NBBY,
smbios_processor_core_flag_name,
smbios_processor_core_flag_desc);
}
if (p.smbp_clkspeed != 0)
oprintf(fp, " External Clock Speed: %uMHz\n", p.smbp_clkspeed);
else
oprintf(fp, " External Clock Speed: Unknown\n");
if (p.smbp_maxspeed != 0)
oprintf(fp, " Maximum Speed: %uMHz\n", p.smbp_maxspeed);
else
oprintf(fp, " Maximum Speed: Unknown\n");
if (p.smbp_curspeed != 0)
oprintf(fp, " Current Speed: %uMHz\n", p.smbp_curspeed);
else
oprintf(fp, " Current Speed: Unknown\n");
id_printf(fp, " L1 Cache: ", p.smbp_l1cache);
id_printf(fp, " L2 Cache: ", p.smbp_l2cache);
id_printf(fp, " L3 Cache: ", p.smbp_l3cache);
}
