public void SetPhysicsPose(Physics3DScene physics3DScene)
{
for (int i = 0; i < rigidBodyDescs.Count; i++)
{
var desc = rigidBodyDescs[i];
if (desc.Type != 0)
{
continue;
}
int index = desc.AssociatedBoneIndex;
Matrix4x4 mat2 = Matrix4x4.CreateFromQuaternion(desc.Rotation) * Matrix4x4.CreateTranslation(desc.Position) * bones[index].GeneratedTransform * LocalToWorld;
physics3DScene.MoveRigidBody(physics3DRigidBodys[i], mat2);
}
}
public void TransformToNew(Physics3DScene physics3DScene, Vector3 position, Quaternion rotation)
{
LocalToWorld = Matrix4x4.CreateFromQuaternion(rotation) * Matrix4x4.CreateTranslation(position);
Matrix4x4.Invert(LocalToWorld, out WorldToLocal);
for (int i = 0; i < rigidBodyDescs.Count; i++)
{
var desc = rigidBodyDescs[i];
if (desc.Type != 0)
{
continue;
}
int index = desc.AssociatedBoneIndex;
var bone = bones[index];
Matrix4x4.Decompose(bone.GeneratedTransform, out _, out Quaternion rot, out Vector3 trans);
Vector3 pos = Vector3.Transform(bone.staticPosition, bone.GeneratedTransform * LocalToWorld);
physics3DScene.MoveRigidBody(physics3DRigidBodys[i], pos, rot);
}
}