public void SetOffsetMatrix(Matrix4x4 offsetMatrix)
{
var temp = offsetMatrix;
offsetTranslate = offsetMatrix.GetColumn(3);
temp.SetColumn(3, Vector4.zero);
offsetRotation = MatrixIO.QuaternionFromMatrix(temp);
}
private void Update()
{
var ip = trackerIp;
var cstMatrix = cstTransformMatrix;
var cstScale = cstScaleFactor;
if (ipFromBoss || cstFromBoss) // If boss is in use
{
var boss = GetTrackerBoss();
if (boss == null)
{
// No boss in scene so avoid checking every frame
enabled = false;
throw new System.Exception("TrackerTransform set to derive from TrackerBoss, but no TrackerBoss in scene. TrackerTransform will be disabled.");
}
if (ipFromBoss)
{
ip = boss.trackerIp;
}
if (cstFromBoss)
{
cstMatrix = boss.CstTransformMatrix;
cstScale = boss.CstScaleFactor;
}
}
// Get tracker pose from VRPN.
var vrpnRot = VRPN_Handler.vrpnTrackerQuat(trackerId + "@" + ip, 0);
var vrpnPos = VRPN_Handler.vrpnTrackerPos(trackerId + "@" + ip, 0);
// Convert from right to left handed coordinate base.
Quaternion lhRot = LHQuatFromRHQuat(vrpnRot, axis);
Vector3 lhPos = LHVectorFromRHVector(vrpnPos, axis);
// Multiply by the CST matrix to transform from tracker worldspace to app worldspace.
var cstRot = MatrixIO.QuaternionFromMatrix(cstMatrix) * lhRot;
var cstPos = cstMatrix.MultiplyPoint3x4(lhPos);
cstPos *= cstScale;
// Apply offset matrix.
var trackerTransform = Matrix4x4.TRS(cstPos, cstRot, Vector3.one);
trackerTransform = trackerTransform * Matrix4x4.TRS(offsetTranslate, offsetRotation, Vector3.one);
// Decompose and apply to GameObject transform.
transform.position = trackerTransform.GetColumn(3);
transform.rotation = MatrixIO.QuaternionFromMatrix(trackerTransform);
}