void World::setTransformFromController(const vector<ControllerBase*>& controllers, bool limit_displacement)
{
assert(cursors.size() == controllers.size());
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
cursor->setTransform(controllers[i]->getPosition(), controllers[i]->getRotation(), limit_displacement);
if (controllers[i]->hasButtonPressedAndReset(UP))
cursor->openClose(limit_displacement);
if (controllers[i]->hasButtonPressedAndReset(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
end_effs[ee_ind]->updateTransformFromAttachment();
}
}
void World::applyRelativeControl(const vector<Control*>& controls, double thresh, bool limit_displacement)
{
assert(cursors.size() == controls.size());
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
Matrix3d rotate(controls[i]->getRotate());
AngleAxisd rotate_aa(rotate);
AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
double trans_norm = controls[i]->getTranslate().norm();
const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm));
const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
if (controls[i]->getButton(UP))
cursor->openClose(limit_displacement);
if (controls[i]->getButton(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
end_effs[ee_ind]->updateTransformFromAttachment();
}
}
//This is hack to easily determine the displacement of each control
void World::applyRelativeControl(const vector<Control*>& controls, vector<double>& displacements, double thresh, bool limit_displacement)
{
assert(cursors.size() == controls.size());
vector<Vector3d> pre_positions;
if ((cursors.size() == 2) || (displacements.size() == 2)) {
pre_positions.resize(2);
for (int i = 0; i < cursors.size(); i++) {
assert(cursors[i]->isAttached());
if (cursors[i]->isAttached())
pre_positions[i] = cursors[i]->end_eff->getPosition();
}
} else {
for (int i = 0; i < displacements.size(); i++) {
displacements[i] = -1;
}
}
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
Matrix3d rotate(controls[i]->getRotate());
AngleAxisd rotate_aa(rotate);
AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
double trans_norm = controls[i]->getTranslate().norm();
const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm));
const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
if (controls[i]->getButton(UP))
cursor->openClose(limit_displacement);
if (controls[i]->getButton(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
end_effs[ee_ind]->updateTransformFromAttachment();
}
if ((cursors.size() == 2) || (displacements.size() == 3)) {
Matrix<double,6,1> u_tran;
for (int i = 0; i < cursors.size(); i++) {
assert(cursors[i]->isAttached());
if (cursors[i]->isAttached())
//cout << "norm of end effector " << i << ": " << (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm() << endl;
displacements[i] = (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm();
u_tran.segment(i*3,3) = (cursors[i]->end_eff->getPosition() - pre_positions[i]);
}
displacements[2] = u_tran.norm();
}
}
