Пример #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);
    }
}
Пример #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);
}
Пример #3
0
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();
}
Пример #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;
}
Пример #5
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();
}
Пример #6
0
void GLView::setViewport(const Camera::Viewport& v)
{
	x=(Real)v.x;
	y=(Real)v.y;
	w=(Real)v.w;
	h=(Real)v.h;
	v.getCameraMatrix(modelview);

	if(v.perspective)
	{
		Real aspect = w/h;
		Real width = v.n*Half/v.scale;
		Real height = v.n*Half/(aspect*v.scale);
		getFrustumMatrix(-width, width, -height, height, v.n, v.f, projection);
	}
	else
	{
		Real width = w*Half/v.scale;
		Real height = h*Half/v.scale;
		getOrthoMatrix(-width, width, -height, height, v.n, v.f, projection);
	}
	updateInverses();
}
Пример #7
0
bool GLView::getViewport(Camera::Viewport& v) const
{
	v.x=(int)x; v.y=(int)y; v.w=(int)w; v.h=(int)h;
	v.setCameraMatrix(modelview);
	const Matrix4& p=projection;

	//now the projmat...
	bool error=false;
	if(p(3,3)==One) {
		//ortho
		v.perspective=false;
		//m00=2/(r-l)=2/(w/scale)=scale*2/w;
		//m11=2/(t-b)=2/(h/scale)=scale*2/h;
		//m22=-2/(f-n)
		//m23=-(f+n)/(f-n)
		//[f*m22-n*m22=-2  ] => [m22   -m22 ] * [f] = [-2]
		//[f*m23-n*m23=-f-n]    [m23+1 -m23+1] * [n]   [0 ]
		v.scale=p(0,0)*(w/2);
		Matrix2 A;
		A(0,0)=p(2,2); A(0,1)=-p(2,2);
		A(1,0)=p(2,3)+1; A(1,1)=-p(2,3)+1;
		if(!A.inplaceInverse()) error=true;
		Vector2 nf;
		A.mul(Vector2(-2,Zero),nf);
		v.n=nf.x;
		v.f=nf.y;
		
#ifdef _DEBUG
		if(v.scale != p(1,1)*h*0.5) error=true;
		  //no projection translation
		if(p(0,3)!=Zero) error=true;
		if(p(1,3)!=Zero) error=true;
		//these ought to be zero
		if(p(3,2)!=Zero) error=true;
		if(p(3,1)!=Zero) error=true;
		if(p(3,0)!=Zero) error=true;
		if(p(2,1)!=Zero) error=true;
		if(p(2,0)!=Zero) error=true;
		if(p(1,0)!=Zero) error=true;
		if(p(1,2)!=Zero) error=true;
		if(p(0,2)!=Zero) error=true;
		if(p(0,1)!=Zero) error=true;
#endif
	}
	else if(p(3,3)==Zero) {
		//perspective
		v.perspective=true;
		//m00=2n/(r-l)=2n/n/scale=2*scale
		//m11=2n/(t-b)=2n/n/(aspect*scale)=2*aspect*scale
		//m22=-(f+n)/(f-n)
		//m23=-2fn/(f-n)
		//f*(m22+1)+n*(-m22+1)=0
		//f*m23 +n*(-m23)=2fn
		//f=n*(m22-1)/(m22+1)=n*c
		//so n*(c*m23-m23)=2*c*n*n
		//c*m23-m23=2*c*n
		v.scale=p(0,0)*Half;
		if(p(2,2)-One != Zero) {
			//solve for n
			Real c=(p(2,2)-1)/(p(2,2)+1);
			v.n=p(2,3)*Half-p(2,3)*Half/c;
			v.f=v.n*c;
		}
		else error=true;

#ifdef _DEBUG
		if(v.scale!=p(1,1)*Half*h/w) error=true;
		//no translation
		if(p(0,2)!=Zero) error=true;
		if(p(1,2)!=Zero) error=true;

		if(p(0,1)!=Zero) error=true;
		if(p(0,3)!=Zero) error=true;
		if(p(1,0)!=Zero) error=true;
		if(p(1,3)!=Zero) error=true;
		if(p(2,0)!=Zero) error=true;
		if(p(2,1)!=Zero) error=true;
		if(p(3,0)!=Zero) error=true;
		if(p(3,1)!=Zero) error=true;
		if(p(3,2)!=-One) error=true;
#endif
	}
	else error=true;
	
	if(error) {
	  fprintf(stderr,"GLView: There was an error in the projection matrix\n");
	  return false;
	}
	return true;
}
Пример #8
0
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();
}
Пример #9
0
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;
}
Пример #10
0
void WorldDragWidget::Drag(int dx,int dy,Camera::Viewport& viewport)
{
  Vector3 v;
  viewport.getMovementVectorAtDistance(dx,dy,hoverDistance,v);
  dragPt += v;
}