private void UpdateHeadAndBodyPose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
// Determine the gaze direction horizontally.
Vector3 gazeDirection = GetHeadForward();
gazeDirection.y = 0;
gazeDirection.Normalize();
//float _bodyLerpFilter = BodyRotationFilter1 (pose);
float _bodyLerpFilter = BodyRotationFilter2(rtPose);
if (_bodyLerpFilter > 0)
{
if (!FollowHead)
{
if (Head == null)
{
Head = WaveVR_Render.Instance.gameObject;
}
if (Head != null)
{
defaultHeadPosition = Head.transform.localPosition;
}
}
}
bodyDirection = Vector3.Slerp(bodyDirection, gazeDirection, _bodyLerpFilter);
bodyRotation = Quaternion.FromToRotation(Vector3.forward, bodyDirection);
}
void Update()
{
// FollowHead changes in runtime.
if (this.FollowHead != this.currFollowHead)
{
this.bSetupFollowHead = false;
}
// SimulationOption changes in runtime.
if (this.SimulationOption != this.currSimulationOption)
{
this.bSetupSimulation = false;
}
SetupPoseSimulation();
if (TrackTiming == WVR_TrackTiming.WhenNewPoses)
{
return;
}
if (!WaveVR.Instance.Initialized)
{
return;
}
WaveVR.Device device = WaveVR.Instance.getDeviceByType(this.Type);
if (device.connected)
{
wvr_pose = device.pose;
rigid_pose = device.rigidTransform;
}
updateDevicePose(wvr_pose, rigid_pose);
}
private void updateControllerPose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
// Place the shoulder in anatomical positions based on the height and handedness.
bodyRotation = Quaternion.identity;
Vector3 v3ControllerAngularVelocity =
new Vector3(pose.pose.AngularVelocity.v0, pose.pose.AngularVelocity.v1, pose.pose.AngularVelocity.v2);
UpdateBodyRotation(v3ControllerAngularVelocity);
ComputeControllerPose2(pose, rtPose);
if (controllerArmModelPosition.y - transform.localPosition.y > 0.8f || transform.localPosition.y - controllerArmModelPosition.y > 0.8f)
{
posePulseCount++;
if (posePulseCount <= 5)
{
Log.w(LOG_TAG, "Skip rendering wrong pose!!");
return;
}
else
{
posePulseCount = 0;
}
}
transform.localPosition = Vector3.Lerp(transform.localPosition, controllerArmModelPosition, smoothMoveSpeed);;
transform.localRotation = controllerArmModelRotation;
}
private void updatePose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
if (trackRotation == true)
{
if (trackPosition == true)
{
updateDevicePose(pose, rtPose);
}
else
{
if (Head != null)
{
switch (type)
{
case WVR_DeviceType.WVR_DeviceType_Controller_Right:
v3ChangeArmXAxis.x = WaveVR_Controller.IsLeftHanded ? -1.0f : 1.0f;
break;
case WVR_DeviceType.WVR_DeviceType_Controller_Left:
v3ChangeArmXAxis.x = WaveVR_Controller.IsLeftHanded ? 1.0f : -1.0f;
break;
default:
break;
}
updateControllerPose(pose, rtPose);
}
}
}
}
private WaveVR()
{
Log.d(LOG_TAG, "WaveVR()+");
WVR_InitError error = Interop.WVR_Init(WVR_AppType.WVR_AppType_VRContent);
if (error != WVR_InitError.WVR_InitError_None)
{
ReportError(error);
Interop.WVR_Quit();
Debug.Log("WVR_Quit");
return;
}
WaveVR_Utils.notifyActivityUnityStarted();
for (int i = 0; i < 3; i++)
{
poses[i] = new WVR_DevicePosePair_t();
connected[i] = false; // force update connection status to all listener.
deviceIndexMap[i] = 0; // use hmd's id as default.
}
hmd = new Device(WVR_DeviceType.WVR_DeviceType_HMD);
controllerLeft = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Left);
controllerRight = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Right);
Log.d(LOG_TAG, "WaveVR()-");
}
private WaveVR()
{
#if UNITY_EDITOR
if (Application.isEditor)
{
try
{
string ipaddr = "";
//WaveVR_Utils.SIM_ConnectType type = WaveVR_Utils.SIM_ConnectType.SIM_ConnectType_USB;
System.IntPtr ptrIPaddr = Marshal.StringToHGlobalAnsi(ipaddr);
WaveVR_Utils.WVR_SetPrintCallback_S(WaveVR_Utils.PrintLog);
WaveVR_Utils.SIM_InitError error = WaveVR_Utils.WVR_Init_S(0, ptrIPaddr);
if (error != 0)
{
WaveVR_Utils.WVR_Quit_S();
return;
}
isSimulatorOn = true;
}
catch (Exception e)
{
return;
}
}
else
#endif
{
Log.d(LOG_TAG, "WaveVR()+");
WVR_InitError error = Interop.WVR_Init(WVR_AppType.WVR_AppType_VRContent);
if (error != WVR_InitError.WVR_InitError_None)
{
ReportError(error);
Interop.WVR_Quit();
return;
}
WaveVR_Utils.notifyActivityUnityStarted();
}
for (int i = 0; i < 3; i++)
{
poses[i] = new WVR_DevicePosePair_t();
connected[i] = false; // force update connection status to all listener.
deviceIndexMap[i] = 0; // use hmd's id as default.
}
hmd = new Device(WVR_DeviceType.WVR_DeviceType_HMD);
controllerLeft = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Left);
controllerRight = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Right);
#if UNITY_EDITOR
if (Application.isEditor)
{
}
else
#endif
{
}
}
private void updateDevicePose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
if (TrackPosition)
{
if (InversePosition)
{
transform.localPosition = -rtPose.pos;
}
else
{
transform.localPosition = rtPose.pos;
}
if (SetCustomHand && pose.pose.Is6DoFPose == false)
{
transform.localPosition = Vector3.Scale(transform.localPosition, v3ChangeArmXAxis);
}
}
if (TrackRotation)
{
if (InverseRotation)
{
transform.localRotation = Quaternion.Inverse(rtPose.rot);
}
else
{
transform.localRotation = rtPose.rot;
}
}
}
private void updateDevicePose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
if (TrackPosition)
{
if (InversePosition)
{
transform.localPosition = -rtPose.pos;
}
else
{
transform.localPosition = rtPose.pos;
}
if (SetCustomHand && pose.pose.Is6DoFPose == false)
{
v3ChangeAxisX.x =
WaveVR_Controller.Input(this.Type).DeviceType == WVR_DeviceType.WVR_DeviceType_Controller_Right ?
1 : -1;
transform.localPosition = Vector3.Scale(transform.localPosition, v3ChangeAxisX);
}
}
if (TrackRotation)
{
if (InverseRotation)
{
transform.localRotation = Quaternion.Inverse(rtPose.rot);
}
else
{
transform.localRotation = rtPose.rot;
}
}
}
void updatePose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
if (trackPosition)
{
if (inversePosition)
{
transform.localPosition = -rtPose.pos;
}
else
{
transform.localPosition = rtPose.pos;
}
}
if (trackRotation)
{
if (inverseRotation)
{
transform.localRotation = Quaternion.Inverse(rtPose.rot);
}
else
{
transform.localRotation = rtPose.rot;
}
}
}
private void updateControllerPose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
// Place the shoulder in anatomical positions based on the height and handedness.
bodyRotation = Quaternion.identity;
UpdateHeadAndBodyPose(pose, rtPose);
ComputeControllerPose2(pose, rtPose);
if (controllerArmModelPosition.y - transform.localPosition.y > 0.8f || transform.localPosition.y - controllerArmModelPosition.y > 0.8f)
{
posePulseCount++;
if (posePulseCount <= 5)
{
Log.w(LOG_TAG, "Skip rendering wrong pose!!");
return;
}
else
{
posePulseCount = 0;
}
}
if (TrackPosition)
{
if (hideController)
{
transform.localPosition = controllerArmModelPosition;
}
else
{
transform.localPosition = Vector3.Lerp(transform.localPosition, controllerArmModelPosition, smoothMoveSpeed);
}
}
if (TrackRotation)
{
transform.localRotation = controllerArmModelRotation;
}
// Skip frame which pose is not updated.
if (hideController)
{
// Since updatePose() is called only when connected, show controller after pose updated.
PrintDebugLog("updateControllerPose() show controller components." +
"\nLocal Pos: " + transform.localPosition.x + ", " + transform.localPosition.y + ", " + transform.localPosition.z +
"\nPos: " + transform.position.x + ", " + transform.position.y + ", " + transform.position.z +
"\nLocal rot: " + transform.localRotation.x + ", " + transform.localRotation.y + ", " + transform.localRotation.z);
ActivateTargetObjects(true);
hideController = false;
}
else
{
Log.gpl.d(LOG_TAG, "Type " + this.Type + ", updateControllerPose()" +
"\nLocal Pos: " + transform.localPosition.x + ", " + transform.localPosition.y + ", " + transform.localPosition.z +
"\nPos: " + transform.position.x + ", " + transform.position.y + ", " + transform.position.z +
"\nLocal rot: " + transform.localRotation.x + ", " + transform.localRotation.y + ", " + transform.localRotation.z);
}
}
public void onLoadLevel()
{
Log.i(LOG_TAG, "onLoadLevel() reset all connection");
for (int i = 0; i < DeviceTypes.Length; i++)
{
poses[i] = new WVR_DevicePosePair_t();
connected[i] = false; // force update connection status to all listener.
}
}
private void updatePose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
switch (this.Type)
{
case WaveVR_Controller.EDeviceType.Dominant:
v3ChangeArmXAxis.x = WaveVR_Controller.IsLeftHanded ? -1.0f : 1.0f;
break;
case WaveVR_Controller.EDeviceType.NonDominant:
v3ChangeArmXAxis.x = WaveVR_Controller.IsLeftHanded ? 1.0f : -1.0f;
break;
default:
break;
}
if (SetCustomHand)
{
v3ChangeArmXAxis.x = (v3ChangeArmXAxis.x == 1.0f) ? -1.0f : 1.0f;
}
#if UNITY_EDITOR
if (Application.isEditor)
{
switch (SimulationOption)
{
case WVR_SimulationOption.NoSimulation:
updateDevicePose(pose, rtPose);
break;
case WVR_SimulationOption.ForceSimulation:
updateControllerPose(pose, rtPose);
break;
case WVR_SimulationOption.WhenNoPosition:
if (pose.pose.Is6DoFPose == false)
{
updateControllerPose(pose, rtPose);
}
else
{
updateDevicePose(pose, rtPose);
}
break;
default:
break;
}
}
else
#endif
{
updateDevicePose(pose, rtPose);
}
}
private Vector3 GetVelocityOffset(WVR_DevicePosePair_t pose)
{
Vector3 velocity = new Vector3(pose.pose.Velocity.v0, pose.pose.Velocity.v1, pose.pose.Velocity.v2);
Vector3 offset = velocity * Time.deltaTime;
offset.x = offset.x == 0 ? 0 : Mathf.Clamp(offset.x + prevVOffset.x, -0.05f, 0.05f);
offset.y = offset.y == 0 ? 0 : Mathf.Clamp(offset.y + prevVOffset.y, -0.1f, 0.1f);
offset.z = offset.z == 0 ? 0 : Mathf.Clamp(offset.z + prevVOffset.z, -0.1f, 0.1f);
prevVOffset = offset;
return(offset);
}
private void updatePose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
switch (this.Type)
{
case WVR_DeviceType.WVR_DeviceType_Controller_Right:
v3ChangeArmXAxis.x = WaveVR_Controller.IsLeftHanded ? -1.0f : 1.0f;
break;
case WVR_DeviceType.WVR_DeviceType_Controller_Left:
v3ChangeArmXAxis.x = WaveVR_Controller.IsLeftHanded ? 1.0f : -1.0f;
break;
default:
break;
}
if (SetCustomHand)
{
v3ChangeArmXAxis.x = (v3ChangeArmXAxis.x == 1.0f) ? -1.0f : 1.0f;
}
if (TrackPosition == false)
{
updateDevicePose(pose, rtPose);
}
else
{
switch (SimulationOption)
{
case WVR_SimulationOption.NoSimulation:
updateDevicePose(pose, rtPose);
break;
case WVR_SimulationOption.ForceSimulation:
updateControllerPose(pose, rtPose);
break;
case WVR_SimulationOption.WhenNoPosition:
if (pose.pose.Is6DoFPose == false)
{
updateControllerPose(pose, rtPose);
}
else
{
updateDevicePose(pose, rtPose);
}
break;
default:
break;
}
}
}
public void onLoadLevel()
{
if (!this.Initialized)
{
return;
}
PrintInfoLog("onLoadLevel() reset all connection");
for (int i = 0; i < DeviceTypes.Length; i++)
{
poses[i] = new WVR_DevicePosePair_t();
isValidPose [i] = false; // force update connection status to all listener.
}
}
private float BodyRotationFilter1(WVR_DevicePosePair_t pose)
{
Vector3 _v3AngularVelocity =
new Vector3(pose.pose.AngularVelocity.v0, pose.pose.AngularVelocity.v1, pose.pose.AngularVelocity.v2);
float _v3magnitude = _v3AngularVelocity.magnitude;
// If magnitude < body angular velocity upper bound, it means body rotation.
// Thus the controller lerp filter will be 0 then controller will not move in scene.
// If magnitude > body angular velocity upper bound, it means controller movement instead of body rotation.
// In order to move controller smoothly, let the lerp max value to 0.2f means controller will move to correct position in 0.5s.
// If controller angular velocity reaches upper bound, it means user wants the controller to move fast!
float _bodyLerpFilter = Mathf.Clamp((_v3magnitude - BodyAngularVelocityUpperBound) / ControllerAngularVelocityUpperBound, 0, 0.2f);
return(_bodyLerpFilter);
}
private WaveVR()
{
PrintInfoLog("WaveVR()+ commit: " + WaveVR_COMMITINFO.wavevr_version);
{
WVR_InitError error = Interop.WVR_Init(WVR_AppType.WVR_AppType_VRContent);
if (error != WVR_InitError.WVR_InitError_None)
{
ReportError(error);
Interop.WVR_Quit();
this.Initialized = false;
PrintErrorLog("WaveVR() initialize simulator failed, WVR_Quit()");
return;
}
WaveVR_Utils.notifyActivityUnityStarted();
}
this.Initialized = true;
PrintInfoLog("WaveVR() initialization succeeded.");
#if !UNITY_EDITOR
UnityPlayerSettingsStereoRenderingPath = WaveVR_Render.IsVRSinglePassBuildTimeSupported()? WaveVR_Render.StereoRenderingPath.SinglePass: WaveVR_Render.StereoRenderingPath.MultiPass;
#else
UnityPlayerSettingsStereoRenderingPath = (WaveVR_Render.StereoRenderingPath)PlayerSettings.stereoRenderingPath;
#endif
PrintInfoLog("UnityPlayerSettingsStereoRenderingPath = " + UnityPlayerSettingsStereoRenderingPath);
for (int i = 0; i < 3; i++)
{
poses[i] = new WVR_DevicePosePair_t();
isValidPose[i] = false; // force update connection status to all listener.
deviceIndexMap[i] = 0; // use hmd's id as default.
}
hmd = new Device(WVR_DeviceType.WVR_DeviceType_HMD);
controllerLeft = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Left);
controllerRight = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Right);
// Check left-handed mode first, then set connection status according to left-handed mode.
SetLeftHandedMode();
UpdateAllConnection();
SetDefaultButtons();
supportedFeatures = Interop.WVR_GetSupportedFeatures();
PrintInfoLog("WaveVR() supportedFeatures: " + supportedFeatures);
PrintInfoLog("WaveVR()-");
}
public void GetTransform(WVR_DeviceType type, ref WVR_DevicePosePair_t pose, ref WaveVR_Utils.RigidTransform rtPose)
{
switch (type)
{
case WVR_DeviceType.WVR_DeviceType_HMD:
pose = pose_head;
rtPose = rtPose_head;
break;
case WVR_DeviceType.WVR_DeviceType_Controller_Right:
pose = pose_right;
rtPose = rtPose_right;
break;
default:
break;
}
}
private void OnNewPoses(params object[] args)
{
WVR_DevicePosePair_t[] _poses = (WVR_DevicePosePair_t[])args [0];
WaveVR_Utils.RigidTransform[] _rtPoses = (WaveVR_Utils.RigidTransform[])args [1];
WVR_DeviceType _type = WaveVR_Controller.Input(this.Type).DeviceType;
for (int i = 0; i < _poses.Length; i++)
{
if (_type == _poses [i].type)
{
wvr_pose = _poses [i];
rigid_pose = _rtPoses [i];
}
}
updateDevicePose(wvr_pose, rigid_pose);
}
void Update()
{
if (timing == WVR_TrackTiming.WhenNewPoses)
{
return;
}
if (!WaveVR.Instance.Initialized)
{
return;
}
WaveVR.Device device = WaveVR.Instance.getDeviceByType(this.type);
if (device.connected)
{
wvr_pose = device.pose;
rigid_pose = device.rigidTransform;
}
updatePose(wvr_pose, rigid_pose);
}
// private uint posePulseCount = 0;
private void updateControllerPose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
bodyRotation = Quaternion.identity;
UpdateHeadAndBodyPose(pose, rtPose);
ComputeControllerPose(pose, rtPose);
/*
* if (controllerSimulatedPosition.y - transform.localPosition.y > 0.8f || transform.localPosition.y - controllerSimulatedPosition.y > 0.8f)
* {
* posePulseCount++;
* if (posePulseCount <= 5)
* {
* Log.w (LOG_TAG, "Skip rendering wrong pose!!");
* return;
* } else
* {
* posePulseCount = 0;
* }
* }
*/
if (TrackPosition)
{
if (WaveVR_Render.Instance.centerWVRCamera != null)
{
controllerSimulatedPosition += WaveVR_Render.Instance.centerWVRCamera.transform.localPosition;
}
transform.localPosition = Vector3.Lerp(transform.localPosition, controllerSimulatedPosition, smoothMoveSpeed);
}
if (TrackRotation)
{
transform.localRotation = controllerSimulatedRotation;
}
Log.gpl.d(LOG_TAG, "Type " + this.Type + ", updateControllerPose()" +
"\nLocal Pos: " + transform.localPosition.x + ", " + transform.localPosition.y + ", " + transform.localPosition.z +
"\nPos: " + transform.position.x + ", " + transform.position.y + ", " + transform.position.z +
"\nLocal rot: " + transform.localRotation.x + ", " + transform.localRotation.y + ", " + transform.localRotation.z);
}
private void DumpDevicePosePair(WVR_DevicePosePair_t InPairPose)
{
// if(InPairPose == null) return;
WVR_PoseState_t tPoseState = InPairPose.pose;
bool tIsValidPose = tPoseState.IsValidPose;
WVR_Matrix4f_t tPoseMatrix = tPoseState.PoseMatrix;
WVR_Vector3f_t tVelocity = tPoseState.Velocity;
WVR_Vector3f_t tAngV = tPoseState.AngularVelocity;
bool tIs6DoFPose = tPoseState.Is6DoFPose;
long tStamp_ns = tPoseState.PoseTimestamp_ns;
WVR_Vector3f_t tAcceleration = tPoseState.Acceleration;
WVR_Vector3f_t tAngAcc = tPoseState.AngularAcceleration;
float tPredictedMilliSec = tPoseState.PredictedMilliSec;
WVR_PoseOriginModel tOriginModel = tPoseState.OriginModel;
WVR_Pose_t tRawPose = tPoseState.RawPose;
WVR_Vector3f_t tPosition = tRawPose.position;
WVR_Quatf_t tRotation = tRawPose.rotation;
Log.d(LOG_TAG, "DevicePosePair:: type=" + InPairPose.type +
",IsValidPose=" + tIsValidPose +
",Stamp_ns=" + tStamp_ns +
",RawPose.Postion(x,y,z)=" + tPosition.v0 + "," + tPosition.v1 + "," + tPosition.v2 +
",RawPose.Rotation(w,x,y,z)=" + tRotation.w + "," + tRotation.x + "," + tRotation.y + "," + tRotation.z +
",Velocity(x,y,z)=" + tVelocity.v0 + "," + tVelocity.v1 + "," + tVelocity.v2 +
",AngularVelocity(x,y,z)=" + tAngV.v0 + "," + tAngV.v1 + "," + tAngV.v2 +
",Acc(x,y,z)=" + tAcceleration.v0 + "," + tAcceleration.v1 + "," + tAcceleration.v2 +
",AngAcc(x,y,z)=" + tAngAcc.v0 + "," + tAngAcc.v1 + "," + tAngAcc.v2 +
",OriginModel=" + tOriginModel +
",PredictedMilliSec=" + tPredictedMilliSec +
",PoseMatrix(4X1)=" + tPoseMatrix.m0 + "," + tPoseMatrix.m1 + "," + tPoseMatrix.m2 + "," + tPoseMatrix.m3 +
",PoseMatrix(4X2)=" + tPoseMatrix.m4 + "," + tPoseMatrix.m5 + "," + tPoseMatrix.m6 + "," + tPoseMatrix.m7 +
",PoseMatrix(4X3)=" + tPoseMatrix.m8 + "," + tPoseMatrix.m9 + "," + tPoseMatrix.m10 + "," + tPoseMatrix.m11 +
",PoseMatrix(4X4)=" + tPoseMatrix.m12 + "," + tPoseMatrix.m13 + "," + tPoseMatrix.m14 + "," + tPoseMatrix.m15 +
".<end>");
}
private void updateDevicePose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
if (TrackPosition)
{
if (InversePosition)
{
transform.localPosition = -rtPose.pos;
}
else
{
transform.localPosition = rtPose.pos;
}
}
if (TrackRotation)
{
if (InverseRotation)
{
transform.localRotation = Quaternion.Inverse(rtPose.rot);
}
else
{
transform.localRotation = rtPose.rot;
}
}
// Skip frame which pose is not updated.
if (hideController)
{
// Since updatePose() is called only when connected, show controller after pose updated.
PrintDebugLog("updateDevicePose() show controller components." +
"\nLocal Pos: " + transform.localPosition.x + ", " + transform.localPosition.y + ", " + transform.localPosition.z +
"\nPos: " + transform.position.x + ", " + transform.position.y + ", " + transform.position.z +
"\nLocal rot: " + transform.localRotation.x + ", " + transform.localRotation.y + ", " + transform.localRotation.z);
ActivateTargetObjects(true);
hideController = false;
}
}
private WaveVR()
{
PrintInfoLog("WaveVR()+ commit: " + WaveVR_COMMITINFO.wavevr_version);
{
WVR_InitError error = Interop.WVR_Init(WVR_AppType.WVR_AppType_VRContent);
if (error != WVR_InitError.WVR_InitError_None)
{
ReportError(error);
Interop.WVR_Quit();
this.Initialized = false;
PrintErrorLog("WaveVR() initialize simulator failed, WVR_Quit()");
return;
}
WaveVR_Utils.notifyActivityUnityStarted();
}
this.Initialized = true;
PrintInfoLog("WaveVR() initialization succeeded.");
for (int i = 0; i < 3; i++)
{
poses[i] = new WVR_DevicePosePair_t();
isValidPose[i] = false; // force update connection status to all listener.
deviceIndexMap[i] = 0; // use hmd's id as default.
}
hmd = new Device(WVR_DeviceType.WVR_DeviceType_HMD);
controllerLeft = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Left);
controllerRight = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Right);
// Check left-handed mode first, then set connection status according to left-handed mode.
SetLeftHandedMode();
UpdateAllConnection();
SetDefaultButtons();
PrintInfoLog("WaveVR()-");
}
// private uint posePulseCount = 0;
private void updateControllerPose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
// Place the shoulder in anatomical positions based on the height and handedness.
bodyRotation = Quaternion.identity;
UpdateHeadAndBodyPose(pose, rtPose);
ComputeControllerPose2(pose, rtPose);
/*
* if (controllerSimulatedPosition.y - transform.localPosition.y > 0.8f || transform.localPosition.y - controllerSimulatedPosition.y > 0.8f)
* {
* posePulseCount++;
* if (posePulseCount <= 5)
* {
* Log.w (LOG_TAG, "Skip rendering wrong pose!!");
* return;
* } else
* {
* posePulseCount = 0;
* }
* }
*/
if (TrackPosition)
{
transform.localPosition = Vector3.Lerp(transform.localPosition, controllerSimulatedPosition, smoothMoveSpeed);
}
if (TrackRotation)
{
transform.localRotation = controllerSimulatedRotation;
}
Log.gpl.d(LOG_TAG, "Type " + this.Type + ", updateControllerPose()" +
"\nLocal Pos: " + transform.localPosition.x + ", " + transform.localPosition.y + ", " + transform.localPosition.z +
"\nPos: " + transform.position.x + ", " + transform.position.y + ", " + transform.position.z +
"\nLocal rot: " + transform.localRotation.x + ", " + transform.localRotation.y + ", " + transform.localRotation.z);
}
public void UpdatePoses(WVR_PoseOriginModel origin, bool isSimulator)
{
if (!this.Initialized)
{
return;
}
Log.gpl.d(LOG_TAG, "UpdatePoses");
#if UNITY_EDITOR
if (Application.isEditor)
{
if (isSimulator)
{
WaveVR_Utils.WVR_GetSyncPose_S((int)origin, poses, poses.Length);
}
}
else
#endif
{
bool _focusCapturedBySystem = Interop.WVR_IsInputFocusCapturedBySystem();
if (this.FocusCapturedBySystem != _focusCapturedBySystem)
{
this.FocusCapturedBySystem = _focusCapturedBySystem;
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.SYSTEMFOCUS_CHANGED, this.FocusCapturedBySystem);
// When getting system focus again, reset button events.
if (!this.FocusCapturedBySystem)
{
Log.i(LOG_TAG, "UpdatePoses() get system focus, reset button events.");
ResetButtonEvents();
}
else
{
Log.i(LOG_TAG, "UpdatePoses() lost system focus.");
}
}
Interop.WVR_GetSyncPose(origin, poses, (uint)poses.Length);
}
for (uint i = 0; i < DeviceTypes.Length; i++)
{
bool _hasType = false;
for (uint j = 0; j < poses.Length; j++)
{
WVR_DevicePosePair_t _pose = poses[j];
if (_pose.type == DeviceTypes [i])
{
_hasType = true;
deviceIndexMap[i] = j;
if (connected [i] != _pose.pose.IsValidPose)
{
connected [i] = _pose.pose.IsValidPose;
Log.i(LOG_TAG, "device " + DeviceTypes [i] + " is " + (connected [i] ? "connected" : "disconnected"));
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.DEVICE_CONNECTED, DeviceTypes [i], connected[i]);
}
if (connected [i])
{
rtPoses[j].update(_pose.pose.PoseMatrix);
}
break;
}
}
// no such type
if (!_hasType)
{
if (connected [i] == true)
{
connected [i] = false;
Log.i(LOG_TAG, "device " + DeviceTypes [i] + " is disconnected.");
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.DEVICE_CONNECTED, DeviceTypes [i], connected[i]);
}
}
}
for (int i = 0; i < poses.Length; i++)
{
WVR_DeviceType _type = poses [i].type;
bool _connected = false;
#if UNITY_EDITOR
if (isSimulator)
{
_connected = WaveVR_Utils.WVR_IsDeviceConnected_S((int)_type);
}
else
#endif
{
_connected = Interop.WVR_IsDeviceConnected(_type);
}
bool _posevalid = poses [i].pose.IsValidPose;
Log.gpl.d(LOG_TAG, "Device " + _type + " is " + (_connected ? "connected" : "disconnected")
+ ", pose is " + (_posevalid ? "valid" : "invalid")
+ ", pos: {" + rtPoses [i].pos.x + ", " + rtPoses [i].pos.y + ", " + rtPoses [i].pos.z + "}"
+ ", rot: {" + rtPoses [i].rot.x + ", " + rtPoses [i].rot.y + ", " + rtPoses [i].rot.z + ", " + rtPoses [i].rot.w + "}");
}
try
{
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.NEW_POSES, poses, rtPoses);
}
catch (Exception ex)
{
Log.e(LOG_TAG, "Send NEW_POSES Event Exception : " + ex);
}
Log.gpl.d(LOG_TAG, "after new poses");
try
{
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.AFTER_NEW_POSES);
}
catch (Exception ex)
{
Log.e(LOG_TAG, "Send AFTER_NEW_POSES Event Exception : " + ex);
}
}
private WaveVR()
{
PrintInfoLog("WaveVR()+ commit: " + WaveVR_COMMITINFO.wavevr_version);
#if UNITY_EDITOR
if (Application.isEditor)
{
this.EnableSimulator = EditorPrefs.GetBool(MENU_NAME, false);
if (this.EnableSimulator)
{
try
{
string ipaddr = "";
//WaveVR_Utils.SIM_ConnectType type = WaveVR_Utils.SIM_ConnectType.SIM_ConnectType_USB;
System.IntPtr ptrIPaddr = Marshal.StringToHGlobalAnsi(ipaddr);
WaveVR_Utils.WVR_SetPrintCallback_S(WaveVR_Utils.PrintLog);
WaveVR_Utils.SIM_InitError error = WaveVR_Utils.WVR_Init_S(0, ptrIPaddr);
PrintInfoLog("WaveVR() WVR_Init_S = " + error);
if (error != 0)
{
WaveVR_Utils.WVR_Quit_S();
PrintErrorLog("WaveVR() initialize simulator failed, WVR_Quit_S()");
return;
}
isSimulatorOn = true;
} catch (Exception e)
{
PrintErrorLog("WaveVR() initialize simulator failed, exception: " + e);
return;
}
}
}
else
#endif
{
WVR_InitError error = Interop.WVR_Init(WVR_AppType.WVR_AppType_VRContent);
if (error != WVR_InitError.WVR_InitError_None)
{
ReportError(error);
Interop.WVR_Quit();
PrintErrorLog("WaveVR() initialize simulator failed, WVR_Quit()");
return;
}
WaveVR_Utils.notifyActivityUnityStarted();
}
this.Initialized = true;
PrintInfoLog("WaveVR() initialization succeeded.");
for (int i = 0; i < 3; i++)
{
poses[i] = new WVR_DevicePosePair_t();
connected[i] = false; // force update connection status to all listener.
deviceIndexMap[i] = 0; // use hmd's id as default.
}
hmd = new Device(WVR_DeviceType.WVR_DeviceType_HMD);
controllerLeft = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Left);
controllerRight = new Device(WVR_DeviceType.WVR_DeviceType_Controller_Right);
// Check left-handed mode first, then set connection status according to left-handed mode.
SetLeftHandedMode();
SetConnectionStatus();
SetDefaultButtons();
PrintInfoLog("WaveVR()-");
}
public void UpdatePoses(WVR_PoseOriginModel origin)
{
Log.gpl.d(LOG_TAG, "UpdatePoses");
Interop.WVR_GetSyncPose(origin, poses, (uint)poses.Length);
for (uint i = 0; i < DeviceTypes.Length; i++)
{
bool _hasType = false;
for (uint j = 0; j < poses.Length; j++)
{
WVR_DevicePosePair_t _pose = poses[j];
if (_pose.type == DeviceTypes [i])
{
_hasType = true;
deviceIndexMap[i] = j;
if (connected [i] != _pose.pose.IsValidPose)
{
connected [i] = _pose.pose.IsValidPose;
Log.i(LOG_TAG, "device " + DeviceTypes [i] + " is " + (connected [i] ? "connected" : "disconnected"));
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.DEVICE_CONNECTED, DeviceTypes [i], connected[i]);
}
if (connected [i])
{
rtPoses[j].update(_pose.pose.PoseMatrix);
}
break;
}
}
// no such type
if (!_hasType)
{
if (connected [i] == true)
{
connected [i] = false;
Log.i(LOG_TAG, "device " + DeviceTypes [i] + " is disconnected.");
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.DEVICE_CONNECTED, DeviceTypes [i], connected[i]);
}
}
}
for (int i = 0; i < poses.Length; i++)
{
WVR_DeviceType _type = poses [i].type;
bool _connected = Interop.WVR_IsDeviceConnected(_type);
bool _posevalid = poses [i].pose.IsValidPose;
Log.gpl.d(LOG_TAG, "Device " + _type + " is " + (_connected ? "connected" : "disconnected")
+ ", pose is " + (_posevalid ? "valid" : "invalid")
+ ", pos: {" + rtPoses [i].pos.x + ", " + rtPoses [i].pos.y + ", " + rtPoses [i].pos.z + "}"
+ ", rot: {" + rtPoses [i].rot.x + ", " + rtPoses [i].rot.y + ", " + rtPoses [i].rot.z + ", " + rtPoses [i].rot.w + "}");
}
try
{
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.NEW_POSES, poses, rtPoses);
}
catch (Exception ex)
{
Log.e(LOG_TAG, "Send NEW_POSES Event Exception : " + ex);
}
Log.gpl.d(LOG_TAG, "after new poses");
try
{
WaveVR_Utils.Event.Send(WaveVR_Utils.Event.AFTER_NEW_POSES);
}
catch (Exception ex)
{
Log.e(LOG_TAG, "Send AFTER_NEW_POSES Event Exception : " + ex);
}
}
private void ComputeControllerPose(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
// Get relative controller orientation
Quaternion controllerRelativeRot = rtPose.rot;
/**
* Controller relative position =
* head position +
* elbow relative position +
* wrist relative position
**/
// -------- 1. get the head position --------
Vector3 controllerRelativePos = GetHeadPosition();
// -------- 2. simulate the elbow relative position --------
Vector3 elbowRelativePos = HEADTOELBOW_OFFSET + new Vector3(0.0f, ElbowRaiseYaxis, ElbowRaiseZaxis);
elbowRelativePos = Vector3.Scale(elbowRelativePos, v3ChangeArmXAxis);
controllerRelativePos += elbowRelativePos;
Vector3 controllerForward = controllerRelativeRot * Vector3.forward;
float controller_angle_pitch = Vector3.Angle(controllerForward, Vector3.up);
controller_angle_pitch = Mathf.Clamp(controller_angle_pitch, ELBOW_ANGLE_PITCH_MIN, ELBOW_ANGLE_PITCH_MAX);
float elbow_angle_pitch = 90.0f - controller_angle_pitch;
float anglePercent_pitch = elbow_angle_pitch < 0 ?
elbow_angle_pitch / ELBOW_ANGLE_PITCH_Y_MIN :
elbow_angle_pitch / ELBOW_ANGLE_PITCH_Y_MAX;
float simulatedRatioY = Mathf.Clamp(anglePercent_pitch, 0.0f, 1.0f);
float wrist_y_axis = elbow_angle_pitch < 0 ?
SIMULATED_OFFSET_Y_MIN * simulatedRatioY :
SIMULATED_OFFSET_Y_MAX * simulatedRatioY;
float controller_angle_yaw = Vector3.Angle(controllerForward, Vector3.right);
controller_angle_yaw = Mathf.Clamp(controller_angle_yaw, ELBOW_ANGLE_YAW_MIN, ELBOW_ANGLE_YAW_MAX);
float elbow_angle_yaw = 90.0f - controller_angle_yaw;
float anglePercent_yaw = elbow_angle_yaw < 0 ?
elbow_angle_yaw / ELBOW_ANGLE_YAW_X_MIN :
elbow_angle_yaw / ELBOW_ANGLE_YAW_X_MAX;
float simulatedRadioX = Mathf.Clamp(anglePercent_yaw, 0.0f, 1.0f);
float wrist_x_axis = elbow_angle_yaw < 0 ?
SIMULATED_OFFSET_X_MIN * simulatedRadioX :
SIMULATED_OFFSET_X_MAX * simulatedRadioX;
// for Z-axis coordinate
float wrist_z_range = elbow_angle_pitch < 0 ?
ELBOWTOWRIST_OFFSET_Z_MAX - ELBOWTOWRIST_OFFSET_Z_MIN_DOWN :
ELBOWTOWRIST_OFFSET_Z_MAX - ELBOWTOWRIST_OFFSET_Z_MIN_UP;
float wrist_z_axis = ELBOWTOWRIST_OFFSET_Z_MAX - (simulatedRatioY * wrist_z_range);
Vector3 wristRelativePos = new Vector3(wrist_x_axis, wrist_y_axis, wrist_z_axis);
controllerRelativePos += wristRelativePos;
controllerRelativePos = bodyRotation * controllerRelativePos;
controllerArmModelPosition = controllerRelativePos;
controllerArmModelRotation = controllerRelativeRot;
}
/// <summary>
/// Get the position of controller in Arm Model
///
/// Consider the parts construct controller position:
/// Parts contain elbow, wrist and controller and each part has default offset from head.
/// 1. simulated elbow offset = default elbow offset apply body rotation = body rotation (Quaternion) * elbow offset (Vector3)
/// 2. simulated wrist offset = default wrist offset apply elbow rotation = elbow rotation (Quaternion) * wrist offset (Vector3)
/// 3. simulated controller offset = default controller offset apply wrist rotation = wrist rotation (Quat) * controller offset (V3)
/// head + 1 + 2 + 3 = controller position.
/// </summary>
/// <param name="pose">WVR_DevicePosePair_t</param>
/// <param name="rtPose">WaveVR_Utils.RigidTransform</param>
private void ComputeControllerPose2(WVR_DevicePosePair_t pose, WaveVR_Utils.RigidTransform rtPose)
{
// if bodyRotation angle is θ, _inverseBodyRation is -θ
// the operator * of Quaternion in Unity means concatenation, not multipler.
// If quaternion qA has angle θ, quaternion qB has angle ε,
// qA * qB will plus θ and ε which means rotating angle θ then rotating angle ε.
// (_inverseBodyRotation * rotation of controller in world space) means angle ε subtracts angle θ.
/// 0. Handle two controller position cases: calculating with head position or calculating without head position case
// initial last head position and last controller rtPose for first time
if (firstPosInit)
{
lastHeadPosition = GetHeadPosition();
lastCtrlrRtPose = rtPose.rot;
noHeadPos = false;
firstPosInit = false;
Log.w(LOG_TAG, "initial last position value.");
}
// If moving controller whose angle is less than the angle bound then calculating controller position without head position.
// Otherwise, calculating controller position with head position
if (Quaternion.Angle(lastCtrlrRtPose, rtPose.rot) <= noHeadPosAngleBound)
{
noHeadPos = true;
}
else
{
lastCtrlrRtPose = rtPose.rot;
noHeadPos = false;
}
Quaternion _controllerRotation = Quaternion.Inverse(bodyRotation) * rtPose.rot;
Vector3 _headPosition;
if (noHeadPos)
{
_headPosition = lastHeadPosition;
}
else
{
_headPosition = GetHeadPosition();
lastHeadPosition = _headPosition;
}
/// 1. simulated elbow offset = default elbow offset apply body rotation = body rotation (Quaternion) * elbow offset (Vector3)
// Default left / right elbow offset.
Vector3 _elbowOffset = Vector3.Scale(HEADTOELBOW_OFFSET, v3ChangeArmXAxis);
// Default left / right elbow pitch offset.
Vector3 _elbowPitchOffset = Vector3.Scale(ELBOW_PITCH_OFFSET, v3ChangeArmXAxis) + new Vector3(0.0f, ElbowRaiseYaxis, ElbowRaiseZaxis);
// Use controller pitch to simulate elbow pitch.
// Range from ELBOW_PITCH_ANGLE_MIN ~ ELBOW_PITCH_ANGLE_MAX.
// The percent of pitch angle will be used to calculate the position offset.
Vector3 _controllerForward = _controllerRotation * Vector3.forward;
float _controllerPitch = 90.0f - Vector3.Angle(_controllerForward, Vector3.up); // 0~90
float _controllerPitchRadio = (_controllerPitch - ELBOW_PITCH_ANGLE_MIN) / (ELBOW_PITCH_ANGLE_MAX - ELBOW_PITCH_ANGLE_MIN);
_controllerPitchRadio = Mathf.Clamp(_controllerPitchRadio, 0.0f, 1.0f);
// According to pitch angle percent, plus offset to elbow position.
_elbowOffset += _elbowPitchOffset * _controllerPitchRadio;
// Apply body rotation and head position to calculate final elbow position.
_elbowOffset = _headPosition + bodyRotation * _elbowOffset;
// Rotation from Z-axis to XY-plane used to simulated elbow & wrist rotation.
Quaternion _controllerXYRotation = Quaternion.FromToRotation(Vector3.forward, _controllerForward);
float _controllerXYRotationRadio = (Quaternion.Angle(_controllerXYRotation, Quaternion.identity)) / 180;
// Simulate the elbow raising curve.
float _elbowCurveLerpValue = ELBOW_TO_XYPLANE_LERP_MIN + (_controllerXYRotationRadio * (ELBOW_TO_XYPLANE_LERP_MAX - ELBOW_TO_XYPLANE_LERP_MIN));
Quaternion _controllerXYLerpRotation = Quaternion.Lerp(Quaternion.identity, _controllerXYRotation, _elbowCurveLerpValue);
/// 2. simulated wrist offset = default wrist offset apply elbow rotation = elbow rotation (Quaternion) * wrist offset (Vector3)
// Default left / right wrist offset
Vector3 _wristOffset = Vector3.Scale(ELBOWTOWRIST_OFFSET, v3ChangeArmXAxis);
// elbow rotation + curve = wrist rotation
// wrist rotation = controller XY rotation
// => elbow rotation + curve = controller XY rotation
// => elbow rotation = controller XY rotation - curve
Quaternion _elbowRotation = bodyRotation * Quaternion.Inverse(_controllerXYLerpRotation) * _controllerXYRotation;
// Apply elbow offset and elbow rotation to calculate final wrist position.
_wristOffset = _elbowOffset + _elbowRotation * _wristOffset;
/// 3. simulated controller offset = default controller offset apply wrist rotation = wrist rotation (Quat) * controller offset (V3)
// Default left / right controller offset.
Vector3 _controllerOffset = Vector3.Scale(WRISTTOCONTROLLER_OFFSET, v3ChangeArmXAxis);
Quaternion _wristRotation = _controllerXYRotation;
// Apply wrist offset and wrist rotation to calculate final controller position.
_controllerOffset = _wristOffset + _wristRotation * _controllerOffset;
controllerArmModelPosition = /*bodyRotation */ _controllerOffset;
controllerArmModelRotation = bodyRotation * _controllerRotation;
}
