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