string IKCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{
if(drag) {
if(currentLink >= 0) {
//alter current desired configuration
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Vector3 wp = currentDestination;
Vector3 ofs;
Vector3 vv;
viewport->getViewVector(vv);
Real d = (wp-viewport->position()).dot(vv);
viewport->getMovementVectorAtDistance(mx,-my,d,ofs);
currentDestination = wp+ofs;
IKGoal goal;
goal.link = currentLink;
goal.localPosition = currentPoint;
goal.SetFixedPosition(currentDestination);
vector<IKGoal> problem(1,goal);
int iters=100;
bool res=SolveIK(*robot,problem,1e-3,iters,0);
command = robot->q;
sendCommand = true;
}
stringstream ss;
ss<<"Drag "<<currentLink<<" "<<currentPoint<<" "<<mx<<" "<<my<<endl;
return ss.str();
}
else {
Ray3D ray;
GetClickRay(mx,my,ray);
Config q;
GetCurConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
int link;
Vector3 localPos;
RobotInfo* rob=world->ClickRobot(ray,link,localPos);
if(rob) {
currentLink = link;
currentPoint = localPos;
currentDestination = robot->links[link].T_World*localPos;
world->robots[0].view.SetGrey();
world->robots[0].view.colors[currentLink].set(1,1,0);
}
else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
return "";
}
}
string JointCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{
if(drag) {
if(currentLink >= 0) {
//alter current desired configuration
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
for(size_t i=0;i<robot->drivers.size();i++) {
if(robot->DoesDriverAffect(i,currentLink)) {
Real val = robot->GetDriverValue(i);
val = Clamp(val+my*0.02,robot->drivers[i].qmin,robot->drivers[i].qmax);
robot->SetDriverValue(i,val);
}
}
command = robot->q;
sendCommand = true;
}
stringstream ss;
ss<<"Drag "<<currentLink<<" "<<mx<<" "<<my<<endl;
return ss.str();
}
else {
Ray3D ray;
GetClickRay(mx,my,ray);
Config q;
GetCurConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
int link;
Vector3 localPos;
RobotInfo* rob=world->ClickRobot(ray,link,localPos);
if(rob) {
currentLink = link;
world->robots[0].view.SetGrey();
world->robots[0].view.colors[currentLink].set(1,1,0);
}
else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
return "";
}
}
void StandardInputProcessor::Hover(int mx,int my)
{
if(move) changed = true;
useSpaceball = false;
move = false;
Ray3D ray;
GetClickRay(mx, my, ray);
int link;
Vector3 localPos;
RobotInfo* rob = world->ClickRobot( ray, link, localPos);
Robot* robot = GetRobot();
if (rob) {
currentLink = link;
currentPoint = localPos;
currentDestination = robot->links[currentLink].T_World*localPos;
rob->view.SetGrey();
rob->view.SetColor(currentLink,GLColor(1, 1, 0));
} else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
}
string PlannerCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{
if(drag) {
printf("Dragging event, dragging status %d\n",(int)dragging);
if(!dragging) {
//first click
Ray3D ray;
GetClickRay(mousex,mousey,ray);
Config q;
GetCurConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
int link;
Vector3 localPos;
RobotInfo* rob=world->ClickRobot(ray,link,localPos);
if(rob) {
currentLink = link;
currentPoint = localPos;
currentDestination = robot->links[link].T_World*localPos;
world->robots[0].view.SetGrey();
world->robots[0].view.colors[currentLink].set(1,1,0);
}
else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
dragging = true;
}
if(currentLink >= 0) {
//alter current desired configuration
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Vector3 wp = currentDestination;
Vector3 ofs;
Vector3 vv;
viewport->getViewVector(vv);
Real d = (wp-viewport->position()).dot(vv);
viewport->getMovementVectorAtDistance(mx,-my,d,ofs);
currentDestination = wp + ofs;
IKGoal goal;
goal.link = currentLink;
goal.localPosition = currentPoint;
goal.SetFixedPosition(currentDestination);
if(planner) {
CartesianObjective* obj=new CartesianObjective(robot);
obj->ikGoal = goal;
planner->Reset(obj);
}
}
stringstream ss;
ss<<"Drag "<<currentLink<<" "<<currentPoint<<" "<<mx<<" "<<my<<endl;
return ss.str();
}
else {
printf("Stop dragging event, dragging status %d\n",(int)dragging);
dragging = false;
mousex=mx;
mousey=my;
return "";
}
}
