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