private void calcHandEyeCalibrationInverse(Matrix4x4 trackingMatrix, CameraType cameraType, out Matrix4x4 outInverted)
{
Vector3 translation;
Quaternion rotation;
MetaioSDKUnity.getHandEyeCalibration(out translation, out rotation, cameraType);
Matrix4x4 rotationMatrix = new Matrix4x4();
NormalizeQuaternion(ref rotation);
rotationMatrix.SetTRS(Vector3.zero,
rotation,
new Vector3(1.0f, 1.0f, 1.0f));
Matrix4x4 translationMatrix = new Matrix4x4();
translationMatrix.SetTRS(translation,
new Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
new Vector3(1.0f, 1.0f, 1.0f));
Matrix4x4 composed = translationMatrix * rotationMatrix;
composed *= trackingMatrix;
outInverted = composed.inverse;
}
private void setCameraOrTrackerPosition(Vector3 p, Quaternion q)
{
// todo, make a function out of this, otherwhise its the same as metaioTracker.cs
Matrix4x4 rotationMatrix = new Matrix4x4();
NormalizeQuaternion(ref q);
rotationMatrix.SetTRS(Vector3.zero,
q,
new Vector3(1.0f, 1.0f, 1.0f));
Matrix4x4 translationMatrix = new Matrix4x4();
translationMatrix.SetTRS(p,
new Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
new Vector3(1.0f, 1.0f, 1.0f));
Matrix4x4 composed = translationMatrix * rotationMatrix;
if (transformCamera)
{
//center the camera in front of goal - z-axis
if (cameraToPositionMono != null && cameraToPositionMono.enabled)
{
Matrix4x4 hecMonoInverse;
calcHandEyeCalibrationInverse(composed, CameraType.Rendering, out hecMonoInverse);
cameraToPositionMono.transform.position = (Vector3)hecMonoInverse.GetColumn(3);
cameraToPositionMono.transform.rotation = QuaternionFromMatrix(hecMonoInverse);
}
if (cameraToPositionLeft != null && cameraToPositionLeft.enabled)
{
Matrix4x4 hecLeftInverse;
calcHandEyeCalibrationInverse(composed, CameraType.RenderingLeft, out hecLeftInverse);
cameraToPositionLeft.transform.position = (Vector3)hecLeftInverse.GetColumn(3);
cameraToPositionLeft.transform.rotation = QuaternionFromMatrix(hecLeftInverse);
}
if (cameraToPositionRight != null && cameraToPositionRight.enabled)
{
Matrix4x4 hecRightInverse;
calcHandEyeCalibrationInverse(composed, CameraType.RenderingRight, out hecRightInverse);
cameraToPositionRight.transform.position = (Vector3)hecRightInverse.GetColumn(3);
cameraToPositionRight.transform.rotation = QuaternionFromMatrix(hecRightInverse);
}
}
else
{
// This only works for mono mode
Vector3 translation;
Quaternion rotation;
MetaioSDKUnity.getHandEyeCalibration(out translation, out rotation, CameraType.Rendering);
transform.position = translation + p;
transform.rotation = rotation * q;
}
}
