BvhTransform[] Interpolate(BvhTransform[] a, BvhTransform[] b, float t)
{
var ret = new BvhTransform[a.Length];
for (var i = 0; i < a.Length; i++)
{
var at = a[i];
var bt = b[i];
var res = new BvhTransform();
res.bone = at.bone;
res.position = Vector3.Lerp(at.position, bt.position, t);
res.rotation = Quaternion.Lerp(at.rotation, bt.rotation, t);
ret[i] = res;
}
return(ret);
}
public BvhTransform[] ReadFrame(int frame)
{
var data = frames[frame];
var ret = new BvhTransform[bones.Length];
for (var i = 0; i < bones.Length; i++)
{
var tf = new BvhTransform();
var bone = bones[i];
tf.bone = bone;
var offset = bone.frameOffset;
if (bone.hasPosition)
{
// Use -'ve X to convert RH->LH
tf.position = new Vector3(-data[offset], data[offset + 1], data[offset + 2]) * 0.01f;
offset += 3;
}
float v1 = data[offset], v2 = data[offset + 1], v3 = data[offset + 2];
Quaternion qx, qy, qz;
switch (bone.rotationOrder)
{
case RotationOrder.XYZ:
qx = Quaternion.AngleAxis(-v1, Vector3.left);
qy = Quaternion.AngleAxis(-v2, Vector3.up);
qz = Quaternion.AngleAxis(-v3, Vector3.forward);
tf.rotation = qx * qy * qz;
break;
case RotationOrder.XZY:
qx = Quaternion.AngleAxis(-v1, Vector3.left);
qy = Quaternion.AngleAxis(-v3, Vector3.up);
qz = Quaternion.AngleAxis(-v2, Vector3.forward);
tf.rotation = qx * qz * qy;
break;
case RotationOrder.YXZ:
qx = Quaternion.AngleAxis(-v2, Vector3.left);
qy = Quaternion.AngleAxis(-v1, Vector3.up);
qz = Quaternion.AngleAxis(-v3, Vector3.forward);
tf.rotation = qy * qx * qz;
break;
case RotationOrder.YZX:
qx = Quaternion.AngleAxis(-v3, Vector3.left);
qy = Quaternion.AngleAxis(-v1, Vector3.up);
qz = Quaternion.AngleAxis(-v2, Vector3.forward);
tf.rotation = qy * qz * qx;
break;
case RotationOrder.ZXY:
qx = Quaternion.AngleAxis(-v2, Vector3.left);
qy = Quaternion.AngleAxis(-v3, Vector3.up);
qz = Quaternion.AngleAxis(-v1, Vector3.forward);
tf.rotation = qz * qx * qy;
break;
case RotationOrder.ZYX:
qx = Quaternion.AngleAxis(-v3, Vector3.left);
qy = Quaternion.AngleAxis(-v2, Vector3.up);
qz = Quaternion.AngleAxis(-v1, Vector3.forward);
tf.rotation = qz * qy * qx;
break;
}
ret[i] = tf;
}
return(ret);
}