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