Exemple #1
0
void RobotPoseWidget::Drag(int dx,int dy,Camera::Viewport& viewport)
{
    if(mode == ModeIKAttach) {
        //printf("Attach dragging, hover widget %d\n",ikPoser.ActiveWidget());
        attachx += dx;
        attachy += dy;
        viewport.getClickSource(attachx,attachy,attachRay.source);
        viewport.getClickVector(attachx,attachy,attachRay.direction);
        double dist;
        bool res=linkPoser.Hover(attachx,attachy,viewport,dist);
        Refresh();
        return;
    }
    else if(mode == ModeIKDelete) {
        return;
    }
    WidgetSet::Drag(dx,dy,viewport);
    if(activeWidget == &basePoser) {
        Robot* robot=linkPoser.robot;
        robot->SetJointByTransform(0,robot->joints[0].linkIndex,basePoser.T);
        robot->UpdateFrames();
        linkPoser.poseConfig = robot->q;
    }
    if(activeWidget == &ikPoser) {
        SolveIK();
    }
    if(activeWidget == &basePoser) {
        SolveIKFixedBase();
    }
    if(activeWidget == &linkPoser) {
        SolveIKFixedJoint(linkPoser.hoverLink);
    }
}
Exemple #2
0
bool RobotLinkPoseWidget::Hover(int x,int y,Camera::Viewport& viewport,double& distance)
{
    Ray3D r;
    viewport.getClickSource(x,y,r.source);
    viewport.getClickVector(x,y,r.direction);
    int oldHoverLink = hoverLink;
    distance = Inf;
    hoverLink = affectedLink = affectedDriver = -1;
    highlightedLinks.resize(0);
    Vector3 worldpt;
    Config oldConfig = robot->q;
    robot->UpdateConfig(poseConfig);
    robot->UpdateGeometry();
    for(size_t i=0; i<robot->links.size(); i++) {
        if(robot->geometry[i].Empty()) continue;
        Real dist;
        if(robot->geometry[i].RayCast(r,&dist)) {
            if(dist < distance) {
                distance = dist;
                Vector3 worldpt = r.source + dist*r.direction;
                robot->links[i].T_World.mulInverse(worldpt,hoverPt);
                hoverLink = i;
            }
        }
    }
    robot->UpdateConfig(oldConfig);
    robot->UpdateGeometry();
    if(hoverLink != -1) {
        //if it's a weld joint, select up the tree to the first movable link
        map<int,int> linkToJoint;
        for(size_t i=0; i<robot->joints.size(); i++) {
            vector<int> inds;
            robot->GetJointIndices(i,inds);
            for(size_t j=0; j<inds.size(); j++)
                linkToJoint[inds[j]]=(int)i;
        }
        int link = hoverLink;
        while(robot->joints[linkToJoint[link]].type == RobotJoint::Weld) {
            highlightedLinks.push_back(link);
            if(robot->parents[link] < 0) break;
            link = robot->parents[link];
        }
        highlightedLinks.push_back(link);
        //get the selected driver
        affectedLink = link;
        for(size_t i=0; i<robot->drivers.size(); i++)
            if(robot->DoesDriverAffect(i,affectedLink)) {
                affectedDriver = (int)i;
                break;
            }
    }
    if(hoverLink != oldHoverLink) Refresh();
    return (hoverLink != -1);
}
void TransformWidget::Drag(int dx,int dy,Camera::Viewport& viewport)
{
  dragX += dx;
  dragY += dy;
  Ray3D r;
  viewport.getClickSource(dragX,dragY,r.source);
  viewport.getClickVector(dragX,dragY,r.direction);
  if(hoverItem < 0) return;
  else if(hoverItem == 0) {
    Vector3 v;
    viewport.getMovementVectorAtDistance(dx,dy,clickDistance,v);
    T.t += v;
  }
  else if(hoverItem <= 3) { //translation
    Line3D axisLine;
    axisLine.source = clickPos;
    axisLine.direction = Vector3(T.R.col(hoverItem-1));
    Real t,u;
    axisLine.closestPoint(r,t,u);
    T.t = clickTransform.t + axisLine.direction*t;
  }
  else {
    Plane3D ringPlane;
    Vector3 axis;
    if(hoverItem <= 6) axis = Vector3(clickTransform.R.col(hoverItem-4));
    else axis = clickAxis;
    Vector3 x,y;
    GetCanonicalBasis(axis,x,y);
    //find rotation to minimize distance from clicked pos to drag ray
    Real cx = x.dot(clickPos - T.t);
    Real cy = y.dot(clickPos - T.t);
    ringPlane.setPointNormal(T.t,axis);
    Real t;
    bool res=ringPlane.intersectsRay(r,&t);
    //odd... no intersection
    if(res==false) return;
    Vector3 raypos = r.source + t*r.direction - T.t;
    Real rx = x.dot(raypos);
    Real ry = y.dot(raypos);
    if(Sqr(rx) + Sqr(ry) < 1e-5) return;
    Real theta = AngleDiff(Atan2(ry,rx),Atan2(cy,cx));
    AngleAxisRotation aa;
    aa.axis = axis;
    aa.angle = theta;
    QuaternionRotation qR,qT,qRes;
    qR.setAngleAxis(aa);
    qT.setMatrix(clickTransform.R);
    qRes.mul(qR,qT);
    qRes.getMatrix(T.R);
  }
  Refresh();
}
Exemple #4
0
bool WorldDragWidget::Hover(int x,int y,Camera::Viewport& viewport,double& distance)
{
  if(!active) return false;
  Ray3D r;
  viewport.getClickSource(x,y,r.source);
  viewport.getClickVector(x,y,r.direction);
  hoverID = -1;
  distance = Inf;
  if(robotsActive) {
    int body;
    Vector3 localpt;
    Robot* rob = world->RayCastRobot(r,body,localpt);
    if(rob) {
      hoverPt = localpt;
      int index = -1;
      for(size_t i=0;i<world->robots.size();i++)
	if(rob == (Robot*)world->robots[i]) { index=(int)i; break; }
      hoverID = world->RobotLinkID(index,body);
      const Geometry::AnyCollisionGeometry3D* geom = world->GetGeometry(hoverID);
      Vector3 worldpt = geom->GetTransform()*localpt;
      distance = worldpt.distance(r.source);
      dragPt = worldpt;
    }
  }
  if(objectsActive) {
    Vector3 localpt;
    RigidObject* obj = world->RayCastObject(r,localpt);
    if(obj) {
      Vector3 worldpt = obj->T*localpt;
      Real d=worldpt.distance(r.source);
      if(d < distance) {
	distance = d;
	hoverPt = localpt;
	int index = -1;
	for(size_t i=0;i<world->rigidObjects.size();i++)
	  if(obj == (RigidObject*)world->rigidObjects[i]) { index=(int)i; break; }
	hoverID = world->RigidObjectID(index);
	dragPt = worldpt;
      }
    }
  }
  hoverDistance = distance;
  if(hoverID >= 0)  {
    return true;
  }
  return false;
}
bool TransformWidget::Hover(int x,int y,Camera::Viewport& viewport,double& distance)
{
  Real globalScale = 1.0;
  if(scaleToScreen) {
    float sx,sy,sz;
    viewport.project(T.t,sx,sy,sz);
    globalScale = sz/viewport.scale;
  }
  distance = Inf;
  int oldHoverItem = hoverItem;
  hoverItem = -1;
  Ray3D r;
  viewport.getClickSource(x,y,r.source);
  viewport.getClickVector(x,y,r.direction);
  //check origin
  if(enableTranslation && enableOriginTranslation) {
    Sphere3D s;
    s.center = T.t;
    s.radius = originRadius*globalScale;
    Real tmin,tmax;
    if(s.intersects(r,&tmin,&tmax)) {
      distance = tmin;
      hoverItem = 0;
    }
  }
  //check translation axes
  for(int i=0;i<3;i++) {
    if(!enableTranslation) break;
    if(!enableTranslationAxes[i]) continue;
    Line3D axisLine;
    axisLine.source = T.t;
    axisLine.direction = Vector3(T.R.col(i));
    Real t,u;
    axisLine.closestPoint(r,t,u);
    t = Clamp(t,0.0,axisLength*globalScale);
    u = Clamp(u,0.0,Inf);
    Vector3 paxis,pray;
    axisLine.eval(t,paxis);
    r.eval(u,pray);
    if(paxis.distanceSquared(pray) <= Sqr(axisRadius*globalScale)) {
      if(u < distance) {
	distance = u;
	hoverItem = 1+i;
      }
    }
  }
  if(enableRotation) {
    //check rotation rings
    Circle3D c;
    c.center = T.t;
    for(int i=0;i<3;i++) {
      if(!enableRotationAxes[i]) continue;
      c.axis = Vector3(T.R.col(i));
      c.radius = ringOuterRadius*globalScale;
      Real t;
      if(c.intersects(r,&t) && t >= 0) {
	c.radius = ringInnerRadius*globalScale;
	if(!c.intersects(r,NULL)) {
	  if(t < distance) {
	    distance = t;
	    hoverItem = i+4;
	  }
	}
      }
    }
  }
  if(enableRotation && enableOuterRingRotation) {
    //check outer ring
    Circle3D c;
    c.center = T.t;
    viewport.getViewVector(c.axis);
    c.radius = (ringOuterRadius+arrowHeight)*globalScale;
    Real t;
    if(c.intersects(r,&t) && t >= 0) {
      c.radius = (ringInnerRadius+arrowHeight)*globalScale;
      if(!c.intersects(r,NULL)) {
	if(t < distance) {
	  distance = t;
	  hoverItem = 7;
	}
      }
    }
    clickAxis = c.axis;
  }
  if(hoverItem != oldHoverItem) Refresh();
  r.eval(distance,hoverPos);
  return hoverItem != -1;
}