public override void SetJointValue(HomogenousTransformation transformation, HomogenousTransformation nextTransformation, Link link, double q)
{
Vector p = nextTransformation.GetPosition();
RootObject.transform.localPosition = p.ToVector3();
RotationMatrix R = nextTransformation.GetRotation();
RootObject.transform.localRotation = R.ToUnityMatrix().rotation;
}
public HomogenousTransformation GetTransformation(double q)
{
if (lastQ == q)
{
return(lastHT);
}
HomogenousTransformation HT;
if (Type == JointType.Revolute)
{
HT = new HomogenousTransformation(new RotationMatrix(q, axe), offset);
}
else
{
HT = new HomogenousTransformation(new RotationMatrix(Matrix.Eye(3).matrix), offset + q * linearMotionDirection);
}
lastHT = HT;
return(HT);
}
public virtual void SetJointValue(HomogenousTransformation transformation, HomogenousTransformation nextTransformation, Link link, double q)
{
}
public override void SetJointValue(HomogenousTransformation transformation, HomogenousTransformation nextTransformation, Link link, double q)
{
RootObject.transform.localPosition = transformation.GetPosition().ToVector3();
RootObject.transform.localRotation = transformation.GetRotation().ToUnityMatrix().rotation;
LinearJoint.transform.localPosition = (link.offset + link.linearMotionDirection * q).ToVector3() / RootObject.transform.localScale.x;
}
public Robot()
{
T_I0 = new HomogenousTransformation(Matrix.Eye(4));
}
