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