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