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 IKGoal::GetClosestGoalTransform(const RigidTransform& T0,RigidTransform& T) const
{
//fill out rotation first
if(rotConstraint == RotFixed) {
GetFixedGoalRotation(T.R);
}
else if(rotConstraint == RotAxis) {
//T.R*localAxis = endRotation
GetMinimalRotation(localAxis,T0.R*endRotation,T.R);
//make it so orthogonal directions perform a rotation similar to T0.R
Vector3 lx,ly,rx,ry,refx;
GetCanonicalBasis(localAxis,lx,ly);
rx = T.R*rx;
ry = T.R*ry;
refx = T0.R*lx;
Real x = dot(refx,rx);
Real y = dot(refx,ry);
//find the rotation about endRotation that gets closer to this
Real theta = Atan2(y,x);
AngleAxisRotation aa;
aa.angle = theta;
aa.axis = T0.R*endRotation;
Matrix3 Rrot;
aa.getMatrix(Rrot);
T.R = Rrot*T.R;
}
else
T.R = T0.R;
T.t = endPosition - T.R*localPosition;
if(posConstraint == PosPlanar) {
//find closest transform on plane to T0.t
Plane3D p;
p.setPointNormal(T.t,direction);
p.project(T0.t,T.t);
}
else if(posConstraint == PosLinear) {
//find closest transform on line to T0.t
Line3D line;
line.source = T.t;
line.direction = direction;
line.closestPoint(T0.t,T.t);
}
else if(posConstraint == PosNone)
T.t = T0.t;
}
void Circle3D::getPlane(Plane3D& p) const
{
p.setPointNormal(center,axis);
}