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; }