// Update the skeleton transforms based on the dragger.
void IvJointDragger::UpdateSkeleton()
{
ItemPtr selectedItem = GetSelectedItem();
if( !selectedItem ) {
return;
}
KinBodyItemPtr pbody = boost::dynamic_pointer_cast<KinBodyItem>(selectedItem);
if( !pbody ) {
return;
}
// save the transform of the object before dragging
SbMatrix mrot;
_trackball->rotation.getValue().getValue(mrot);
float fang = -atan2f(mrot[2][1], mrot[1][1]);
// if a robot, reset the controller
RobotItemPtr probotitem = boost::dynamic_pointer_cast<RobotItem>(pbody);
{
EnvironmentMutex::scoped_try_lock lock(_penv->GetMutex());
if( !!lock ) {
KinBody::JointPtr pjoint = pbody->GetBody()->GetJoints()[_iJointIndex];
int d = pjoint->GetDOFIndex();
vector<dReal> vlower,vupper;
pbody->GetBody()->GetDOFLimits(vlower, vupper);
if( pjoint->GetType() == KinBody::JointSlider ) {
fang = fang*(vupper.at(d)-vlower.at(d))+vlower.at(d);
}
// update the joint's transform
vector<dReal> vjoints;
pbody->GetBody()->GetDOFValues(vjoints);
// double check all the limits
for(size_t i = 0; i < vjoints.size(); ++i) {
if( vjoints[i] < vlower[i] ) {
vjoints[i] = vlower[i];
}
else if( vjoints[i] > vupper[i] ) {
vjoints[i] = vupper[i];
}
}
if( pjoint->GetType() == KinBody::JointSpherical ) {
SbVec3f axis; float angle;
_trackball->rotation.getValue(axis,angle);
vjoints.at(d+0) = axis[0]*angle;
vjoints.at(d+1) = axis[1]*angle;
vjoints.at(d+2) = axis[2]*angle;
}
else {
vjoints.at(d+0) = fang+_jointoffset;
if( pjoint->IsRevolute(0) ) {
if( vjoints.at(d) < vlower.at(d) ) {
if( vlower.at(d)-vjoints.at(d) < vjoints.at(d)+2*PI-vupper.at(d) ) {
vjoints[d] = vlower[d];
}
else {
vjoints[d] = vupper[d];
}
}
else if( vjoints.at(d) > vupper.at(d) ) {
if( vlower.at(d)-vjoints.at(d)+2*PI < vjoints.at(d)-vupper.at(d) ) {
vjoints[d] = vlower[d];
}
else {
vjoints[d] = vupper[d];
}
}
}
else {
if( vjoints.at(d) < vlower.at(d) ) {
vjoints[d] = vlower[d];
}
else if( vjoints.at(d) > vupper.at(d) ) {
vjoints[d] = vupper[d];
}
}
}
if( !!probotitem && !!probotitem->GetRobot()->GetController() ) {
probotitem->GetRobot()->GetController()->SetDesired(vjoints);
}
else {
pbody->GetBody()->SetDOFValues(vjoints, KinBody::CLA_CheckLimits);
}
}
}
_viewer.lock()->_UpdateCameraTransform(0);
UpdateDragger();
// do collision checking if required
CheckCollision(_checkCollision);
selectedItem->SetGrab(false, false); // will be updating manually
pbody->UpdateFromModel();
}
