/**
* The global axis for this joint takes the orientation of the inboard link into account; thus, if the orientation
* of the inboard link changes, then the global axis changes.
* \sa getAxisLocal()
* \sa setAxisLocal()
*/
Vector3 SphericalJoint::get_axis_global(Axis a) const
{
const unsigned X = 0;
// get the inboard link
RigidBodyPtr inboard_link = get_inboard_link();
// make sure that the inboard link has been set
if (!inboard_link)
{
std::cerr << "SphericalJoint::get_axis_global() - attempt to get axis w/o inboard link!" << std::endl;
return ZEROS_3;
}
// get the transform for the inboard link
Matrix3 R;
inboard_link->get_transform().get_rotation(&R);
// axis one is easy
if (a == eAxis1)
{
Vector3 axis;
_R.get_column(X, axis.begin());
return R * axis;
}
// for both axes 2 and 3 we need cos and sin of q(1)
const Real c1 = std::cos(q[DOF_1]+_q_tare[DOF_1]);
const Real s1 = std::sin(q[DOF_1]+_q_tare[DOF_1]);
// axis two is obtained by multiplying rotation matrix around x by axis [0,1,0]
if (a == eAxis2)
return R * _R * Vector3(0,c1,s1);
// axis 3, requires the rotation matrix induced by the axis-angle
// representation for axis 2.. much simpler to just use the rotation matrix from the
// universal joint induced transform
const Real c2 = std::cos(q[DOF_2]+_q_tare[DOF_2]);
const Real s2 = std::sin(q[DOF_2]+_q_tare[DOF_2]);
assert (a == eAxis3);
return R * _R * Vector3(s2, -c2*s1, c1*c2);
}
void histogram(const Vector1& input, // assumed to be already sorted
Vector2& histogram_values,
Vector3& histogram_counts) {
typedef typename Vector1::value_type ValueType; // input value type
typedef typename Vector3::value_type IndexType; // histogram index type
thrust::device_vector<ValueType> data(input);
IndexType num_bins = thrust::inner_product(data.begin(), data.end() - 1,
data.begin() + 1,
IndexType(1),
thrust::plus<IndexType>(),
thrust::not_equal_to<ValueType>());
histogram_values.resize(num_bins);
histogram_counts.resize(num_bins);
BOOST_LOG_TRIVIAL(debug) << boost::format("num_bins: %d") % num_bins;
thrust::reduce_by_key(data.begin(), data.end(),
thrust::constant_iterator<IndexType>(1),
histogram_values.begin(),
histogram_counts.begin());
}