virtual void PreEval(Real x) { AngleAxisRotation aa; aa.angle = x; aa.axis = z; aa.getMatrix(temp); temp = temp*R; }
Vector3 AxisSweptPoint::eval(Real theta) const { Vector3 ploc=p-axis.source; AngleAxisRotation aa; aa.axis=axis.direction; aa.angle=theta; Vector3 temp; aa.transformPoint(ploc,temp); return temp+axis.source; }
void SO3CSpace::SampleNeighborhood(const Config& c,Real r,Config& x) { AngleAxisRotation aa; aa.angle = Rand(0,r); SampleSphere(1.0,aa.axis); Matrix3 R,dR; aa.getMatrix(R); GetRotation(c,R); SetRotation(R*dR,x); }
Real SO3CSpace::Distance(const Config& a,const Config& b) { Matrix3 Ra,Rb; GetRotation(a,Ra); GetRotation(b,Rb); Matrix3 Rrel; Rrel.mulTransposeB(Ra,Rb); AngleAxisRotation aa; aa.setMatrix(Rrel); return Abs(aa.angle); }
void IKGoal::GetEdgeGoalTransform(Real theta,RigidTransform& T) const { Matrix3 R0,RTheta; GetBaseEdgeRotation(R0); AngleAxisRotation aa; aa.axis = localAxis; aa.angle = theta; aa.getMatrix(RTheta); T.R.mul(R0,RTheta); T.t = endPosition - T.R*localPosition; }
virtual void Eval(Real t,Vector& v) { Matrix3 Ra,ARaB; AngleAxisRotation aa; aa.angle=t; aa.axis=a; aa.getMatrix(Ra); ARaB = A*Ra*B; MomentRotation m; m.setMatrix(ARaB); v.resize(3); m.get(&v(0)); }
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; }
virtual void Deriv(Real t,Vector& dv) { Matrix3 Ra,ARaB; AngleAxisRotation aa; aa.angle=t; aa.axis=a; aa.getMatrix(Ra); ARaB = A*Ra*B; Vector3 z,dm; A.mul(a,z); MomentDerivative(ARaB,z,dm); /* MomentRotation m; m.setMatrix(ARaB); MomentDerivative(m,z,dm); */ dv.resize(3); dm.get(&dv(0)); }
virtual Real Eval(Real x) { AngleAxisRotation aa; aa.setMatrix(temp); return aa.angle; }