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();
}
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 WorldDragWidget::Drag(int dx,int dy,Camera::Viewport& viewport)
{
Vector3 v;
viewport.getMovementVectorAtDistance(dx,dy,hoverDistance,v);
dragPt += v;
}