Exemple #1
0
void RobotLinkPoseWidget::Drag(int dx,int dy,Camera::Viewport& viewport)
{
    if(affectedDriver < 0) return;
    robot->UpdateConfig(poseConfig);
    //this is for rotational joints
    Real shift = dy*0.02;
    //for prismatic joints, use the distance in space
    if(robot->drivers[affectedDriver].linkIndices.size()==1) {
        int link = robot->drivers[affectedDriver].linkIndices[0];
        if(robot->links[link].type == RobotLink3D::Prismatic) {
            Vector3 pt = robot->links[link].T_World.t;
            float x,y,d;
            viewport.project(pt,x,y,d);
            Vector3 v;
            viewport.getMovementVectorAtDistance(0,dy,d,v);
            shift = -Sign(Real(dy))*v.norm();
        }
    }

    Real val = Clamp(robot->GetDriverValue(affectedDriver)+shift,robot->drivers[affectedDriver].qmin,robot->drivers[affectedDriver].qmax);
    robot->SetDriverValue(affectedDriver,val);
    poseConfig = robot->q;
    Refresh();
}
void TransformWidget::DrawGL(Camera::Viewport& viewport)
{
  glEnable(GL_LIGHTING);
  Real scale=1.0;
  Real globalScale = 1.0;
  if(scaleToScreen) {
    float sx,sy,sz;
    viewport.project(T.t,sx,sy,sz);
    globalScale = sz/viewport.scale;
  }

  glPushMatrix();
  glTranslate(T.t);
  //draw origin
  if(enableTranslation && enableOriginTranslation) {
    scale = (hasHighlight && hoverItem == 0 ? hoverScale : 1.0)*globalScale;
    glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,originColor.rgba); 
    drawSphere(originRadius*scale,16,8);
  }
  else {
    glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,originColor.rgba); 
    drawSphere(axisRadius*globalScale,16,8);
  }

  //draw axes
  if(enableTranslation) {
    Vector3 axis;
    if(enableTranslationAxes[0]) {
      axis = T.R.col1();
      scale = (hasHighlight && hoverItem == 1 ? hoverScale : 1.0)*globalScale;
      glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,xAxisColor.rgba); 
      drawCylinder(axis*axisLength*globalScale,axisRadius*scale,8);
      glPushMatrix();
      glTranslate(axis*axisLength*globalScale);
      drawCone(axis*arrowHeight*scale,arrowRadius*scale);
      glPopMatrix();
    }

    if(enableTranslationAxes[1]) {
      axis = T.R.col2();    
      scale = (hasHighlight && hoverItem == 2 ? hoverScale : 1.0)*globalScale;
      glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,yAxisColor.rgba); 
      drawCylinder(axis*axisLength*globalScale,axisRadius*scale,8);
      glPushMatrix();
      glTranslate(axis*axisLength*globalScale);
      drawCone(axis*arrowHeight*scale,arrowRadius*scale);
      glPopMatrix();
    }

    if(enableTranslationAxes[2]) {
      axis = T.R.col3();
      scale = (hasHighlight && hoverItem == 3 ? hoverScale : 1.0)*globalScale;
      glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,zAxisColor.rgba); 
      drawCylinder(axis*axisLength*globalScale,axisRadius*scale,8);
      glPushMatrix();
      glTranslate(axis*axisLength*globalScale);
      drawCone(axis*arrowHeight*scale,arrowRadius*scale);
      glPopMatrix();
    }
  }

  //TODO: indicate original matrix rotation

  //draw rings
  if(enableRotation) {
    glDisable(GL_CULL_FACE);
    Vector3 x,y;
    Vector3 axis;
    Real r1,r2;
    if (enableRotationAxes[0]) {
      axis = Vector3(T.R.col1()); 
      GetCanonicalBasis(axis,x,y);
      r1 = (hasHighlight && hoverItem == 4 ? ringInnerRadius - (ringOuterRadius - ringInnerRadius)*0.5*hoverScale : ringInnerRadius)*globalScale;
      r2 = (hasHighlight && hoverItem == 4 ? ringOuterRadius + (ringOuterRadius - ringInnerRadius)*0.5*hoverScale : ringOuterRadius)*globalScale;
      glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,xAxisColor.rgba); 
      drawArc(r1,r2,axis,x,0,360);
    }
    
    if (enableRotationAxes[1]) {
      axis = Vector3(T.R.col2()); 
      GetCanonicalBasis(axis,x,y);
      r1 = (hasHighlight && hoverItem == 5 ? ringInnerRadius - (ringOuterRadius - ringInnerRadius)*0.5*hoverScale : ringInnerRadius)*globalScale;
      r2 = (hasHighlight && hoverItem == 5 ? ringOuterRadius + (ringOuterRadius - ringInnerRadius)*0.5*hoverScale : ringOuterRadius)*globalScale;
      glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,yAxisColor.rgba); 
      drawArc(r1,r2,axis,x,0,360);
    }

    if (enableRotationAxes[2]) {
      axis = Vector3(T.R.col3()); 
      GetCanonicalBasis(axis,x,y);
      r1 = (hasHighlight && hoverItem == 6 ? ringInnerRadius - (ringOuterRadius - ringInnerRadius)*0.5*hoverScale : ringInnerRadius)*globalScale;
      r2 = (hasHighlight && hoverItem == 6 ? ringOuterRadius + (ringOuterRadius - ringInnerRadius)*0.5*hoverScale : ringOuterRadius)*globalScale;
      glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,zAxisColor.rgba); 
      drawArc(r1,r2,axis,x,0,360);
    }

    if(enableOuterRingRotation && hoverItem == 7) {
      axis = clickAxis;
      GetCanonicalBasis(axis,x,y);
      r1 = (ringInnerRadius+arrowHeight)*globalScale;
      r2 = (ringOuterRadius+arrowHeight)*globalScale;
      float color[4] = {0,0,0,1};
      glMaterialfv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,color); 
      drawArc(r1,r2,axis,x,0,360);
    }
    glEnable(GL_CULL_FACE);
  }

  glPopMatrix();
}
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;
}