Beispiel #1
0
 virtual void PreEval(Real x) {
   AngleAxisRotation aa;
   aa.angle = x;
   aa.axis = z;
   aa.getMatrix(temp);
   temp = temp*R;
 }
Beispiel #2
0
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);
}
Beispiel #5
0
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;
}
Beispiel #6
0
  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));
  }
Beispiel #7
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;
}
Beispiel #8
0
  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));
  }
Beispiel #9
0
 virtual Real Eval(Real x) {
   AngleAxisRotation aa;
   aa.setMatrix(temp);
   return aa.angle;
 }