public RigidPose GetInverse()
{
var t = new RigidPose(pos, rot);
t.Inverse();
return(t);
}
public static void SetPose(Transform target, RigidPose pose, Transform origin = null)
{
if (origin != null && origin != target.parent)
{
target.position = origin.transform.TransformPoint(pose.pos);
target.rotation = origin.rotation * pose.rot;
}
else
{
target.localPosition = pose.pos;
target.localRotation = pose.rot;
}
}
public static void SetPose(Transform target, RigidPose pose, Transform origin = null)
{
if (origin != null && origin != target.parent)
{
pose = new RigidPose(origin) * pose;
pose.pos.Scale(origin.localScale);
target.position = pose.pos;
target.rotation = pose.rot;
}
else
{
target.localPosition = pose.pos;
target.localRotation = pose.rot;
}
}
public void Multiply(RigidPose a, RigidPose b)
{
rot = a.rot * b.rot;
pos = a.pos + a.rot * b.pos;
}
public static RigidPose FromToPose(RigidPose from, RigidPose to)
{
var invRot = Quaternion.Inverse(from.rot);
return(new RigidPose(invRot * (to.pos - from.pos), invRot * to.rot));
}
public void LerpUnclamped(RigidPose to, float t)
{
pos = Vector3.LerpUnclamped(pos, to.pos, t);
rot = Quaternion.SlerpUnclamped(rot, to.rot, t);
}
public static RigidPose LerpUnclamped(RigidPose a, RigidPose b, float t)
{
return(new RigidPose(Vector3.LerpUnclamped(a.pos, b.pos, t), Quaternion.SlerpUnclamped(a.rot, b.rot, t)));
}
public JointPose(RigidPose pose)
{
this.isValid = true;
this.pose = pose;
}
public JointPose(Vector3 position, Quaternion rotation)
{
this.isValid = true;
this.pose = new RigidPose(position, rotation);
}