/// Returns the local translation for a bone, scaled according to its scale and the scale of the bones above it
/// in the heirarchy, apart from the root translation
public Output_GetSegmentLocalTranslation GetScaledSegmentTranslation(string SubjectName, string SegmentName)
{
double[] OutputScale = new double[3];
OutputScale[0] = OutputScale[1] = OutputScale[2] = 1.0;
// Check first whether we have a parent, as we don't wish to scale the root node's position
Output_GetSegmentParentName Parent = GetSegmentParentName(SubjectName, SegmentName);
string CurrentSegmentName = SegmentName;
if (Parent.Result == Result.Success)
{
do
{
// We have a parent. First get our scale, and then iterate through the nodes above us
Output_GetSegmentStaticScale Scale = GetSegmentScale(SubjectName, CurrentSegmentName);
if (Scale.Result == Result.Success)
{
for (uint i = 0; i < 3; ++i)
{
if (Scale.Scale[i] != 0.0)
{
OutputScale[i] = OutputScale[i] * Scale.Scale[i];
}
}
}
Parent = GetSegmentParentName(SubjectName, CurrentSegmentName);
if (Parent.Result == Result.Success)
{
CurrentSegmentName = Parent.SegmentName;
}
} while (Parent.Result == Result.Success);
}
Output_GetSegmentLocalTranslation Translation = GetSegmentTranslation(SubjectName, SegmentName);
if (Translation.Result == Result.Success)
{
for (uint i = 0; i < 3; ++i)
{
Translation.Translation[i] = Translation.Translation[i] / OutputScale[i];
}
}
return(Translation);
}
void Update()
{
Output_GetSubjectRootSegmentName OGSRSN = Client.GetSubjectRootSegmentName(ObjectName);
string SegRootName = OGSRSN.SegmentName;
// UNITY-49 - Don't apply root motion to parent object
Transform Root = transform;
if (Root == null)
{
throw new Exception("fbx doesn't have root");
}
Output_GetSegmentLocalRotationQuaternion ORot = Client.GetSegmentRotation(ObjectName, SegRootName);
Output_GetSegmentLocalTranslation OTran = Client.GetSegmentTranslation(ObjectName, SegRootName);
if (ORot.Result == Result.Success && OTran.Result == Result.Success && !OTran.Occluded)
{
// Input data is in Vicon co-ordinate space; z-up, x-forward, rhs.
// We need it in Unity space, y-up, z-forward lhs
// Vicon Unity
// forward x z
// up z y
// right -y x
// See https://gamedev.stackexchange.com/questions/157946/converting-a-quaternion-in-a-right-to-left-handed-coordinate-system
Root.localRotation = new Quaternion((float)ORot.Rotation[1], -(float)ORot.Rotation[2], -(float)ORot.Rotation[0], (float)ORot.Rotation[3]);
Root.localPosition = new Vector3(-(float)OTran.Translation[1] * 0.001f, (float)OTran.Translation[2] * 0.001f, (float)OTran.Translation[0] * 0.001f);
m_LastGoodPosition = Root.localPosition;
m_LastGoodRotation = Root.localRotation;
m_bHasCachedPose = true;
}
else
{
if (m_bHasCachedPose)
{
Root.localRotation = m_LastGoodRotation;
Root.localPosition = m_LastGoodPosition;
}
}
}
private void ApplyBoneTransform(Transform Bone)
{
// update the bone transform from the data stream
Output_GetSegmentLocalRotationQuaternion ORot = MyClient.GetSegmentLocalRotationQuaternion(SubjectName, Bone.gameObject.name);
if (ORot.Result == Result.Success)
{
Bone.localRotation = new Quaternion(-(float)ORot.Rotation[0], (float)ORot.Rotation[1], (float)ORot.Rotation[2], -(float)ORot.Rotation[3]);
}
Output_GetSegmentLocalTranslation OTran = MyClient.GetSegmentLocalTranslation(SubjectName, Bone.gameObject.name);
if (OTran.Result == Result.Success)
{
Bone.localPosition = new Vector3(-(float)OTran.Translation[0], (float)OTran.Translation[1], (float)OTran.Translation[2]);
}
// recurse through children
for (int iChild = 0; iChild < Bone.childCount; iChild++)
{
ApplyBoneTransform(Bone.GetChild(iChild));
}
}
void LateUpdate()
{
try
{
double Time = HMDUtils.FusionService.GetTime();
uint ViconFrameNumber = Client.GetFrameNumber();
//position
Output_GetSubjectRootSegmentName RootName = Client.GetSubjectRootSegmentName(HmdName);
Output_GetSegmentLocalTranslation Translation = Client.GetSegmentTranslation(HmdName, RootName.SegmentName);
// Raw Vicon position, scale is in mm. The data here is in the datastream default; x-forward, y-left, z-up for the global coordinate system
HMDUtils.FusionService.Vec ViconPosition = new HMDUtils.FusionService.Vec(Translation.Translation[0], Translation.Translation[1], Translation.Translation[2]);
//orientation. The local coordinate system of the HMD object is x-right, y-up, z-back
Output_GetSegmentLocalRotationQuaternion Rot = Client.GetSegmentRotation(HmdName, RootName.SegmentName);
// Raw Vicon orientation
HMDUtils.FusionService.Quat ViconOrientation = new HMDUtils.FusionService.Quat(Rot.Rotation[0], Rot.Rotation[1], Rot.Rotation[2], Rot.Rotation[3]);
// If we don't get a result, or the pose returned from the datastream is occluded, then we will use the last known good position that we received.
bool bViconPoseValid = true;
if (Rot.Result != ViconDataStreamSDK.CSharp.Result.Success || Rot.Occluded || Translation.Occluded)
{
// We use this flag to determine whether to initialize the fusion algorithm; we don't want to initialize it on occluded frames
bViconPoseValid = false;
if (m_LastGoodPose != null)
{
ViconPosition = m_LastGoodPose.Position;
ViconOrientation = m_LastGoodPose.Rotation;
}
else
{
// If all else fails, we will return the origin :(
ViconOrientation = new HMDUtils.FusionService.Quat(0, 0, 0, 1);
}
}
else
{
if (m_LastGoodPose == null)
{
m_LastGoodPose = new HMDUtils.FusionService.Pose(ViconPosition, ViconOrientation);
}
else
{
m_LastGoodPose.Position = ViconPosition;
m_LastGoodPose.Rotation = ViconOrientation;
}
}
//Oculus space. We need to translate to the oculus coordinate system here so that the fusion algorithm can work with all data in the same coordinate system.
// The Vicon data comes in as z-up, x-forward rhs. We convert to y-up, z-back rhs. The local coordinate system of the tracked Oculus object in the Vicon data is already y-up, z-back.
// The conversion also scales from mm to m.
// Vicon Oculus Unity
// forward x -z z
// up z y y
// right -y x x
// https://gamedev.stackexchange.com/questions/157946/converting-a-quaternion-in-a-right-to-left-handed-coordinate-system
//HMDUtils.FusionService.Quat ViconOrientationInOculus = new HMDUtils.FusionService.Quat(-ViconOrientation.Y, ViconOrientation.Z, -ViconOrientation.X, ViconOrientation.W);
//HMDUtils.FusionService.Vec ViconPositionInOculus = new HMDUtils.FusionService.Vec(-ViconPosition.Y * 0.001, ViconPosition.Z * 0.001, -ViconPosition.X * 0.001);
//HMDUtils.FusionService.Pose ViconInOculus = new HMDUtils.FusionService.Pose( ViconPositionInOculus, ViconOrientationInOculus );
// For XR, convert the vicon data to Unity coordinate system
//HMDUtils.FusionService.Quat Rotation = new HMDUtils.FusionService.Quat(ViconOrientation.Y, -ViconOrientation.Z, -ViconOrientation.X, ViconOrientation.W);
//HMDUtils.FusionService.Vec Position = new HMDUtils.FusionService.Vec(-ViconPosition.Y * 0.001, ViconPosition.Z * 0.001, ViconPosition.X * 0.001);
//HMDUtils.FusionService.Pose ViconInUnity = new HMDUtils.FusionService.Pose(Position, Rotation);
// Otherwise convert to Oculus
HMDUtils.FusionService.Pose ViconInOculus = HMDUtils.FusionService.GetMappedVicon(ViconOrientation, ViconPosition);
bool bOK = false;
HMDUtils.FusionService.Quat HmdOrtOculus = new HMDUtils.FusionService.Quat(0, 0, 0, 0);
HMDUtils.FusionService.Vec HmdOrtVOculus = new HMDUtils.FusionService.Vec(0, 0, 0);
HMDUtils.FusionService.Vec HmdOrtAOculus = new HMDUtils.FusionService.Vec(0, 0, 0);
HMDUtils.FusionService.Vec HmdPosOculus = new HMDUtils.FusionService.Vec(0, 0, 0);
HMDUtils.FusionService.Vec HmdPosVOculus = new HMDUtils.FusionService.Vec(0, 0, 0);
HMDUtils.FusionService.Vec HmdPosAOculus = new HMDUtils.FusionService.Vec(0, 0, 0);
// Not sure whether we actually require this, plus XR doesn't give it.
double HmdTime = 0;
if (!XRCalls)
{
// We have to assume this
bOK = true;
// The pose from the oculus; this is already in oculus coordinate system - y - up, z - back, rhs
HMDUtils.OVRPluginServices.PoseStatef HMDState = HMDUtils.OVRPluginServices.ovrp_GetNodePoseState(HMDUtils.OVRPluginServices.Step.Render, HMDUtils.OVRPluginServices.Node.EyeCenter);
HmdOrtOculus = Adapt(HMDState.Pose.Orientation);
HmdOrtVOculus = Adapt(HMDState.AngularVelocity);
HmdOrtAOculus = Adapt(HMDState.AngularAcceleration);
HmdPosOculus = Adapt(HMDState.Pose.Position);
HmdPosVOculus = Adapt(HMDState.Velocity);
HmdPosAOculus = Adapt(HMDState.Acceleration);
HmdTime = HMDState.Time;
}
else
{
List <UnityEngine.XR.XRNodeState> XRNodeStates = new List <UnityEngine.XR.XRNodeState>();
UnityEngine.XR.InputTracking.GetNodeStates(XRNodeStates);
foreach (var State in XRNodeStates)
{
if (State.nodeType == UnityEngine.XR.XRNode.CenterEye)
{
Quaternion Ort = new Quaternion();
Vector3 OrtV = new Vector3();
Vector3 OrtA = new Vector3();
Vector3 Pos = new Vector3();
Vector3 PosV = new Vector3();
Vector3 PosA = new Vector3();
bOK = State.TryGetRotation(out Ort);
// If this is present, we will use it. If not, we will calculate it.
if (!State.TryGetAngularVelocity(out OrtV))
{
OrtV = Vector3.zero;
}
// We don't use these, but continue to obtain them for logging purposes
State.TryGetAngularAcceleration(out OrtA);
State.TryGetPosition(out Pos);
State.TryGetVelocity(out PosV);
State.TryGetAcceleration(out PosA);
if (bOK)
{
HmdOrtOculus = Adapt(Ort);
HmdOrtVOculus = Adapt(OrtV);
HmdOrtAOculus = Adapt(OrtA);
HmdPosOculus = Adapt(Pos);
HmdPosVOculus = Adapt(PosV);
HmdPosAOculus = Adapt(PosA);
}
}
}
}
if (bOK && m_Service != null)
{
HMDUtils.FusionService.Quat Output;
bool bOutputOk = m_Service.GetUpdatedOrientation(Time, HmdOrtOculus, HmdOrtVOculus, ViconInOculus.Rotation, ViconInOculus.Position, bViconPoseValid, (float)1, (float)0.0137, (float)0.00175, out Output);
if (bOutputOk)
{
if (Log)
{
m_Log.WriteLine("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18},{19},{20},{21},{22},{23},{24},{25},{26},{27},{28},{29},{30},{31},{32},{33},{34},{35}",
Time, HmdTime,
HmdOrtOculus.X, HmdOrtOculus.Y, HmdOrtOculus.Z, HmdOrtOculus.W,
HmdOrtVOculus.X, HmdOrtVOculus.Y, HmdOrtVOculus.Z,
HmdOrtAOculus.X, HmdOrtAOculus.Y, HmdOrtAOculus.Z,
HmdPosOculus.X, HmdPosOculus.Y, HmdPosOculus.Z,
HmdPosVOculus.X, HmdPosVOculus.Y, HmdPosVOculus.Z,
HmdPosAOculus.X, HmdPosAOculus.Y, HmdPosAOculus.Z,
ViconFrameNumber,
ViconInOculus.Rotation.X, ViconInOculus.Rotation.Y, ViconInOculus.Rotation.Z, ViconInOculus.Rotation.W,
ViconInOculus.Position.X, ViconInOculus.Position.Y, ViconInOculus.Position.Z,
ViconOrientation.X, ViconOrientation.Y, ViconOrientation.Z, ViconOrientation.W,
ViconPosition.X, ViconPosition.Y, ViconPosition.Z);
}
Quaternion OutputOrt = new Quaternion((float)-Output.X, (float)-Output.Y, (float)Output.Z, (float)Output.W);
Quaternion OculusOrt = new Quaternion((float)-HmdOrtOculus.X, (float)-HmdOrtOculus.Y, (float)HmdOrtOculus.Z, (float)HmdOrtOculus.W);
transform.localPosition = new Vector3((float)ViconInOculus.Position.X, (float)ViconInOculus.Position.Y, (float)-ViconInOculus.Position.Z);
transform.localRotation = OutputOrt * Quaternion.Inverse(OculusOrt);
}
}
}
catch (DllNotFoundException ex)
{
Debug.LogError(string.Format("XR must be enabled for this project to use the HMD fusion script: Error {0}", ex.Message));
}
}
void LateUpdate()
{
try
{
double Time = HMDUtils.FusionService.GetTime();
uint ViconFrameNumber = Client.GetFrameNumber();
//position
Output_GetSubjectRootSegmentName RootName = Client.GetSubjectRootSegmentName(HmdName);
Output_GetSegmentLocalTranslation Translation = Client.GetSegmentTranslation(HmdName, RootName.SegmentName);
// Raw Vicon position, scale is in mm. The data here is in the datastream default; x-forward, y-left, z-up for the global coordinate system
HMDUtils.FusionService.Vec ViconPosition = new HMDUtils.FusionService.Vec(Translation.Translation[0], Translation.Translation[1], Translation.Translation[2]);
//orientation. The local coordinate system of the HMD object is x-right, y-up, z-back
Output_GetSegmentLocalRotationQuaternion Rot = Client.GetSegmentRotation(HmdName, RootName.SegmentName);
// Raw Vicon orientation
HMDUtils.FusionService.Quat ViconOrientation = new HMDUtils.FusionService.Quat(Rot.Rotation[0], Rot.Rotation[1], Rot.Rotation[2], Rot.Rotation[3]);
// If we don't get a result, or the pose returned from the datastream is occluded, then we will use the last known good position that we received.
bool bViconPoseValid = true;
if (Rot.Result != ViconDataStreamSDK.CSharp.Result.Success || Rot.Occluded || Translation.Occluded)
{
// We use this flag to determine whether to initialize the fusion algorithm; we don't want to initialize it on occluded frames
bViconPoseValid = false;
if (m_LastGoodPose != null)
{
ViconPosition = m_LastGoodPose.Position;
ViconOrientation = m_LastGoodPose.Rotation;
}
else
{
// If all else fails, we will return the origin :(
ViconOrientation = new HMDUtils.FusionService.Quat(0, 0, 0, 1);
}
}
else
{
if (m_LastGoodPose == null)
{
m_LastGoodPose = new HMDUtils.FusionService.Pose(ViconPosition, ViconOrientation);
}
else
{
m_LastGoodPose.Position = ViconPosition;
m_LastGoodPose.Rotation = ViconOrientation;
}
}
//Oculus space. We need to translate to the oculus coordinate system here so that the fusion algorithm can work with all data in the same coordinate system.
// The Vicon data comes in as z-up, x-forward rhs. We convert to y-up, z-back rhs. The local coordinate system of the tracked Oculus object in the Vicon data is already y-up, z-back.
// The conversion also scales from mm to m.
// Vicon Oculus Unity
// forward x -z z
// up z y y
// right -y x x
//HMDUtils.FusionService.Quat ViconOrientationInOculus = new HMDUtils.FusionService.Quat(-ViconOrientation.Y, ViconOrientation.Z, -ViconOrientation.X, ViconOrientation.W);
//HMDUtils.FusionService.Vec ViconPositionInOculus = new HMDUtils.FusionService.Vec(-ViconPosition.Y * 0.001, ViconPosition.Z * 0.001, -ViconPosition.X * 0.001);
//HMDUtils.FusionService.Pose ViconInOculus = new HMDUtils.FusionService.Pose( ViconPositionInOculus, ViconOrientationInOculus );
HMDUtils.FusionService.Pose ViconInOculus = HMDUtils.FusionService.GetMappedVicon(ViconOrientation, ViconPosition);
// The pose from the oculus; this is already in oculus coordinate system - y-up, z-back, rhs
HMDUtils.OVRPluginServices.PoseStatef HMDState = HMDUtils.OVRPluginServices.ovrp_GetNodePoseState(HMDUtils.OVRPluginServices.Step.Render, HMDUtils.OVRPluginServices.Node.EyeCenter);
HMDUtils.OVRPluginServices.Quatf HmdOrt = HMDState.Pose.Orientation;
HMDUtils.OVRPluginServices.Vector3f HmdOrtV = HMDState.AngularVelocity;
HMDUtils.OVRPluginServices.Vector3f HmdOrtA = HMDState.AngularAcceleration;
HMDUtils.OVRPluginServices.Vector3f HmdPos = HMDState.Pose.Position;
HMDUtils.OVRPluginServices.Vector3f HmdPosV = HMDState.Velocity;
HMDUtils.OVRPluginServices.Vector3f HmdPosA = HMDState.Acceleration;
if (m_Service != null)
{
HMDUtils.FusionService.Quat Output;
if (m_Service.GetUpdatedOrientation(Time,
Adapt(HmdOrt),
Adapt(HmdOrtV),
Adapt(HmdOrtA),
ViconInOculus.Rotation,
ViconInOculus.Position,
bViconPoseValid,
(float)1,
(float)0.0137,
(float)0.00175,
out Output))
{
if (Log)
{
m_Log.WriteLine("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18},{19},{20},{21},{22},{23},{24},{25},{26},{27},{28},{29},{30},{31},{32},{33},{34},{35}",
Time, HMDState.Time,
HmdOrt.x, HmdOrt.y, HmdOrt.z, HmdOrt.w,
HmdOrtV.x, HmdOrtV.y, HmdOrtV.z,
HmdOrtA.x, HmdOrtA.y, HmdOrtA.z,
HmdPos.x, HmdPos.y, HmdPos.z,
HmdPosV.x, HmdPosV.y, HmdPosV.z,
HmdPosA.x, HmdPosA.y, HmdPosA.z,
ViconFrameNumber,
ViconInOculus.Rotation.X, ViconInOculus.Rotation.Y, ViconInOculus.Rotation.Z, ViconInOculus.Rotation.W,
ViconInOculus.Position.X, ViconInOculus.Position.Y, ViconInOculus.Position.Z,
ViconOrientation.X, ViconOrientation.Y, ViconOrientation.Z, ViconOrientation.W,
ViconPosition.X, ViconPosition.Y, ViconPosition.Z);
}
// We are in Oculus co-ordinate space: y-up, z-backward rhs. We need to convert to Unity, which is y-up, z-forward lhs - eg a reflection in the xy plane
Quaternion OutputOrt = new Quaternion((float)-Output.X, (float)-Output.Y, (float)Output.Z, (float)Output.W);
Quaternion OculusOrt = new Quaternion((float)-HmdOrt.x, (float)-HmdOrt.y, (float)HmdOrt.z, (float)HmdOrt.w);
transform.localPosition = new Vector3((float)ViconInOculus.Position.X, (float)ViconInOculus.Position.Y, (float)-ViconInOculus.Position.Z);
transform.localRotation = OutputOrt * Quaternion.Inverse(OculusOrt);
}
}
}
catch (DllNotFoundException ex)
{
Debug.LogError(string.Format("XR must be enabled for this project to use the HMD fusion script: Error {0}", ex.Message));
}
}
public override PoseDataFlags GetPoseFromProvider(out Pose output)
{
try
{
double Time = HMDUtils.FusionService.GetTime();
uint ViconFrameNumber = Client.GetFrameNumber();
//position
Output_GetSubjectRootSegmentName RootName = Client.GetSubjectRootSegmentName(HmdName);
Output_GetSegmentLocalTranslation Translation = Client.GetSegmentTranslation(HmdName, RootName.SegmentName);
// Raw Vicon position, scale is in mm. The data here is in the datastream default; x-forward, y-left, z-up for the global coordinate system
HMDUtils.FusionService.Vec ViconPosition = new HMDUtils.FusionService.Vec(Translation.Translation[0], Translation.Translation[1], Translation.Translation[2]);
//orientation. The local coordinate system of the HMD object is x-right, y-up, z-back
Output_GetSegmentLocalRotationQuaternion Rot = Client.GetSegmentRotation(HmdName, RootName.SegmentName);
// Raw Vicon orientation
HMDUtils.FusionService.Quat ViconOrientation = new HMDUtils.FusionService.Quat(Rot.Rotation[0], Rot.Rotation[1], Rot.Rotation[2], Rot.Rotation[3]);
// If we don't get a result, or the pose returned from the datastream is occluded, then we will use the last known good position that we received.
bool bViconPoseValid = true;
if (Rot.Result != ViconDataStreamSDK.CSharp.Result.Success || Rot.Occluded || Translation.Occluded)
{
// We use this flag to determine whether to initialize the fusion algorithm; we don't want to initialize it on occluded frames
bViconPoseValid = false;
if (m_LastGoodPose != null)
{
ViconPosition = m_LastGoodPose.Position;
ViconOrientation = m_LastGoodPose.Rotation;
}
else
{
// If all else fails, we will return the origin :(
ViconOrientation = new HMDUtils.FusionService.Quat(0, 0, 0, 1);
}
}
else
{
if (m_LastGoodPose == null)
{
m_LastGoodPose = new HMDUtils.FusionService.Pose(ViconPosition, ViconOrientation);
}
else
{
m_LastGoodPose.Position = ViconPosition;
m_LastGoodPose.Rotation = ViconOrientation;
}
}
// to headset
HMDUtils.FusionService.Pose ViconInHMD = HMDUtils.FusionService.GetMappedVicon(ViconOrientation, ViconPosition);
// headset to unity
ViconOrientation = ViconInHMD.Rotation;
ViconPosition = ViconInHMD.Position;
HMDUtils.FusionService.Quat Rotation = new HMDUtils.FusionService.Quat(-ViconOrientation.X, -ViconOrientation.Y, ViconOrientation.Z, ViconOrientation.W);
HMDUtils.FusionService.Vec Position = new HMDUtils.FusionService.Vec(ViconPosition.X, ViconPosition.Y, -ViconPosition.Z);
HMDUtils.FusionService.Pose ViconInUnity = new HMDUtils.FusionService.Pose(Position, Rotation);
HMDUtils.FusionService.Quat HmdOrtUnity = new HMDUtils.FusionService.Quat(0, 0, 0, 1);
HMDUtils.FusionService.Vec HmdOrtVUnity = new HMDUtils.FusionService.Vec(0, 0, 0);
HMDUtils.FusionService.Vec HmdOrtAUnity = new HMDUtils.FusionService.Vec(0, 0, 0);
HMDUtils.FusionService.Vec HmdPosUnity = new HMDUtils.FusionService.Vec(0, 0, 0);
HMDUtils.FusionService.Vec HmdPosVUnity = new HMDUtils.FusionService.Vec(0, 0, 0);
HMDUtils.FusionService.Vec HmdPosAUnity = new HMDUtils.FusionService.Vec(0, 0, 0);
List <UnityEngine.XR.XRNodeState> XRNodeStates = new List <UnityEngine.XR.XRNodeState>();
UnityEngine.XR.InputTracking.GetNodeStates(XRNodeStates);
bool bOK = false;
foreach (var State in XRNodeStates)
{
if (State.nodeType == UnityEngine.XR.XRNode.CenterEye)
{
Quaternion Ort = new Quaternion();
Vector3 OrtV = new Vector3();
Vector3 OrtA = new Vector3();
Vector3 Pos = new Vector3();
Vector3 PosV = new Vector3();
Vector3 PosA = new Vector3();
bOK = State.TryGetRotation(out Ort);
// If this is present, we will use it. If not, we will calculate it.
if (!State.TryGetAngularVelocity(out OrtV))
{
OrtV = Vector3.zero;
}
// We don't use these, but continue to obtain them for logging purposes
State.TryGetAngularAcceleration(out OrtA);
State.TryGetPosition(out Pos);
State.TryGetVelocity(out PosV);
State.TryGetAcceleration(out PosA);
if (bOK)
{
HmdOrtUnity = Adapt(Ort);
HmdOrtVUnity = Adapt(OrtV);
HmdOrtAUnity = Adapt(OrtA);
HmdPosUnity = Adapt(Pos);
HmdPosVUnity = Adapt(PosV);
HmdPosAUnity = Adapt(PosA);
}
}
}
// Not sure whether we actually require this, plus XR doesn't give it.
double HmdTime = 0;
if (m_Service != null)
{
HMDUtils.FusionService.MathUtilsError FusionState = HMDUtils.FusionService.MathUtilsError.ENoVelocity;
double V = HMDUtils.FusionService.ScalarVelocity(HmdOrtVUnity);
if (V == 0 || bAlwaysCalculateVelocity)
{
bRequiredVelocityCalculation = true;
FusionState = m_Service.GetVelocity(HmdOrtUnity, Time, out V);
}
else
{
bRequiredVelocityCalculation = false;
FusionState = HMDUtils.FusionService.MathUtilsError.ESuccess;
}
HMDUtils.FusionService.Quat Output = new HMDUtils.FusionService.Quat(0, 0, 0, 1);
if (FusionState == HMDUtils.FusionService.MathUtilsError.ESuccess)
{
if (bAltAlgo)
{
FusionState = m_Service.GetUpdatedOrientationWindowed(Time, HmdOrtUnity, true, ViconInUnity.Rotation, 0, true, out Output);
}
else
{
FusionState = m_Service.GetUpdatedOrientation(Time, HmdOrtUnity, V, ViconInUnity.Rotation, bViconPoseValid, out Output);
}
}
if (FusionState != HMDUtils.FusionService.MathUtilsError.ESuccess)
{
//fall back to vicon rotation
Output = ViconInUnity.Rotation;
}
if (Log)
{
m_Log.WriteLine("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18},{19},{20},{21},{22},{23},{24},{25},{26},{27},{28},{29},{30},{31},{32},{33},{34},{35},{36}",
Time, HmdTime,
HmdOrtUnity.X, HmdOrtUnity.Y, HmdOrtUnity.Z, HmdOrtUnity.W,
HmdOrtVUnity.X, HmdOrtVUnity.Y, HmdOrtVUnity.Z,
HmdOrtAUnity.X, HmdOrtAUnity.Y, HmdOrtAUnity.Z,
HmdPosUnity.X, HmdPosUnity.Y, HmdPosUnity.Z,
HmdPosVUnity.X, HmdPosVUnity.Y, HmdPosVUnity.Z,
HmdPosAUnity.X, HmdPosAUnity.Y, HmdPosAUnity.Z,
ViconFrameNumber,
ViconInUnity.Rotation.X, ViconInUnity.Rotation.Y, ViconInUnity.Rotation.Z, ViconInUnity.Rotation.W,
ViconInUnity.Position.X, ViconInUnity.Position.Y, ViconInUnity.Position.Z,
ViconOrientation.X, ViconOrientation.Y, ViconOrientation.Z, ViconOrientation.W,
ViconPosition.X, ViconPosition.Y, ViconPosition.Z,
HMDUtils.FusionService.StateInString(FusionState, bRequiredVelocityCalculation));
}
if (FusionState == HMDUtils.FusionService.MathUtilsError.ESuccess)
{
output = new Pose(Adapt(ViconInUnity.Position), Adapt(Output));
return(PoseDataFlags.Position | PoseDataFlags.Rotation);
}
}
if (m_LastGoodPose == null)
{
output = new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1));
return(PoseDataFlags.NoData);
}
else
{
output = new Pose(Adapt(m_LastGoodPose.Position), Adapt(m_LastGoodPose.Rotation));
Debug.LogWarning("using last postion");
return(PoseDataFlags.Position | PoseDataFlags.Rotation);
}
}
catch (DllNotFoundException ex)
{
Debug.LogError(string.Format("XR must be enabled for this project to use the HMD fusion script: Error {0}", ex.Message));
output = new Pose(Adapt(m_LastGoodPose.Position), Adapt(m_LastGoodPose.Rotation));
return(PoseDataFlags.NoData);
}
}
