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