private void UpdatePosition()
{
AssignTransformationMatrices();
Matrix4x4 m1 = S.inverse * XRR * CO * RHC * user1.oPrime;
Matrix4x4 m2 = S.inverse * XRR * CO * LHC * user2.oPrime;
// Debug.Log(user1.selectedobjetMatrix);
// Debug.Log(m1);
t1 = user1.GrabScaling(new Vector3(m1[0, 3], m1[1, 3], m1[2, 3]));
t2 = user2.GrabScaling(new Vector3(m2[0, 3], m2[1, 3], m2[2, 3]));
Vector3 originalPosition = new Vector3(oPrime[0, 3], oPrime[1, 3], oPrime[2, 3]);
float l1 = Vector3.Distance(t1, originalPosition);
float l2 = Vector3.Distance(t2, originalPosition);
float w1 = 0.5f;
float w2 = 0.5f;
// if (l1 > l2) {
// // w1 = 0.25f * (((l1 - l2) / l2) + 0.5f);
// }
// else {
// // w1 = 0.25f * (((l2 - l1) / l1) + 0.5f);
// }
w1 = (l1 / (l1 + l2));
w2 = 1f - w1;
Debug.Log("w1: " + w1);
Debug.Log("w2: " + w2);
Vector3 t1Scaled = (t1 - originalPosition) * w1;
Vector3 t2Scaled = (t2 - originalPosition) * w2;
translation = t1Scaled + t2Scaled;
translation = originalPosition + translation;
user1.possitionDiff = t1 - translation;
user2.possitionDiff = t2 - translation;
Quaternion r1 = m1.rotation;
Quaternion r2 = m2.rotation;
Quaternion total = Quaternion.Slerp(r1, r2, 1 / 2f);
user1.rotationDiff = total.eulerAngles;
user1.lastRotation = r1.eulerAngles;
user2.rotationDiff = total.eulerAngles;
user2.lastRotation = r2.eulerAngles;
selectedObject.transform.localPosition = translation;
selectedObject.transform.localRotation = total;
}
