public void update(WVR_Matrix4f_t pose)
{
var m = toMatrix44(pose);
this.pos = m.GetPosition();
this.rot = m.GetRotation();
}
public static Matrix4x4 ToMatrix44(WVR_Matrix4f_t pose)
{
var m = Matrix4x4.identity;
m[0, 0] = pose.m0;
m[0, 1] = pose.m1;
m[0, 2] = pose.m2;
m[0, 3] = pose.m3;
m[1, 0] = pose.m4;
m[1, 1] = pose.m5;
m[1, 2] = pose.m6;
m[1, 3] = pose.m7;
m[2, 0] = pose.m8;
m[2, 1] = pose.m9;
m[2, 2] = pose.m10;
m[2, 3] = pose.m11;
m[3, 0] = pose.m12;
m[3, 1] = pose.m13;
m[3, 2] = pose.m14;
m[3, 3] = pose.m15;
return(m);
}
public RigidTransform(WVR_Matrix4f_t pose)
{
var m = toMatrix44(pose);
this.pos = m.GetPosition();
this.rot = m.GetRotation();
}
public static Matrix4x4 toMatrix44(WVR_Matrix4f_t pose, bool glToUnity = true)
{
var m = Matrix4x4.identity;
int sign = glToUnity ? -1 : 1;
m[0, 0] = pose.m0;
m[0, 1] = pose.m1;
m[0, 2] = pose.m2 * sign;
m[0, 3] = pose.m3;
m[1, 0] = pose.m4;
m[1, 1] = pose.m5;
m[1, 2] = pose.m6 * sign;
m[1, 3] = pose.m7;
m[2, 0] = pose.m8 * sign;
m[2, 1] = pose.m9 * sign;
m[2, 2] = pose.m10;
m[2, 3] = pose.m11 * sign;
m[3, 0] = pose.m12;
m[3, 1] = pose.m13;
m[3, 2] = pose.m14;
m[3, 3] = pose.m15;
return(m);
}
private void UpdateRightPose(float axis_x, float axis_y, float axis_z)
{
// Right-Alt + mouse for x & y angle.
if (Input.GetKey(KeyCode.RightAlt))
{
rightAngleY += axis_x / 2;
rightAngleX -= axis_y * 1.5f;
}
// Right-Ctrl + mouse for z angle.
if (Input.GetKey(KeyCode.RightControl))
{
rightAngleZ += axis_z * 5;
}
rightRotation = Quaternion.Euler(rightAngleX, rightAngleY, rightAngleZ);
// Right-Shift + mouse for position.
if (Input.GetKey(KeyCode.RightShift))
{
rightPosX += axis_x / 5;
rightPosY += axis_y / 5;
rightPosZ += axis_z;
}
//-------- keyboard control ---------
if (Input.GetKey(KeyCode.RightArrow))
{
rightPosX += shiftSpeed_Right * Time.deltaTime;
}
if (Input.GetKey(KeyCode.LeftArrow))
{
rightPosX -= shiftSpeed_Right * Time.deltaTime;
}
if (Input.GetKey(KeyCode.UpArrow))
{
rightPosY += shiftSpeed_Right * Time.deltaTime;
}
if (Input.GetKey(KeyCode.DownArrow))
{
rightPosY -= shiftSpeed_Right * Time.deltaTime;
}
if (simulationType == WVR_SimulationType.WVR_SimulationType_ForceOff ||
(simulationType == WVR_SimulationType.WVR_SimulationType_Auto && is6DoFPose == true)
)
{
rightPosition.x = rightPosX;
rightPosition.y = rightPosY;
rightPosition.z = rightPosZ;
}
if (simulationType == WVR_SimulationType.WVR_SimulationType_ForceOn ||
(simulationType == WVR_SimulationType.WVR_SimulationType_Auto && is6DoFPose == false)
)
{
updateControllerPose(WVR_DeviceType.WVR_DeviceType_Controller_Right, rightRigidTransform);
rightPosition = Vector3.Lerp(rightPosition, controllerSimulatedPosition, smoothMoveSpeed);
}
rightRigidTransform.update(rightPosition, rightRotation);
rightPoseMatrix = GetOpenGLMatrix44(rightPosition, rightRotation);
}
private void DumpPoseState(WVR_PoseState_t tPoseState)
{
// if(tPoseState == null || !tPoseState.IsValidPose ) return;
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, "PoseState:: 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 DumpWVR_Matrix4f(WVR_Matrix4f_t tPoseMatrix)
{
Log.d(LOG_TAG, "WVR_Matrix4f=" +
"(4X1)={" + tPoseMatrix.m0 + "," + tPoseMatrix.m1 + "," + tPoseMatrix.m2 + "," + tPoseMatrix.m3 + "}" +
"(4X2)={" + tPoseMatrix.m4 + "," + tPoseMatrix.m5 + "," + tPoseMatrix.m6 + "," + tPoseMatrix.m7 + "}" +
"(4X3)={" + tPoseMatrix.m8 + "," + tPoseMatrix.m9 + "," + tPoseMatrix.m10 + "," + tPoseMatrix.m11 + "}" +
"(4X4)={" + tPoseMatrix.m12 + "," + tPoseMatrix.m13 + "," + tPoseMatrix.m14 + "," + tPoseMatrix.m15 + "}" +
".<end>");
}
public void OnIpdChanged(params object[] args)
{
Log.d(LOG_TAG, "configurationChanged");
#if UNITY_EDITOR
if (Application.isEditor)
{
return;
}
#endif
WVR_NumDoF dof;
if (WaveVR.Instance.is6DoFTracking() == 3)
{
dof = WVR_NumDoF.WVR_NumDoF_3DoF;
}
else
{
if (origin == WVR_PoseOriginModel.WVR_PoseOriginModel_OriginOnHead_3DoF)
{
dof = WVR_NumDoF.WVR_NumDoF_3DoF;
}
else
{
dof = WVR_NumDoF.WVR_NumDoF_6DoF;
}
}
//for update EyeToHead transform
WVR_Matrix4f_t eyeToHeadL = Interop.WVR_GetTransformFromEyeToHead(WVR_Eye.WVR_Eye_Left, dof);
WVR_Matrix4f_t eyeToHeadR = Interop.WVR_GetTransformFromEyeToHead(WVR_Eye.WVR_Eye_Right, dof);
eyes = new WaveVR_Utils.RigidTransform[] {
new WaveVR_Utils.RigidTransform(eyeToHeadL),
new WaveVR_Utils.RigidTransform(eyeToHeadR)
};
//for update projection matrix
Interop.WVR_GetClippingPlaneBoundary(WVR_Eye.WVR_Eye_Left, ref projRawL[0], ref projRawL[1], ref projRawL[2], ref projRawL[3]);
Interop.WVR_GetClippingPlaneBoundary(WVR_Eye.WVR_Eye_Right, ref projRawR[0], ref projRawR[1], ref projRawR[2], ref projRawR[3]);
Log.d(LOG_TAG, "targetFPS=" + targetFPS + " sceneWidth=" + sceneWidth + " sceneHeight=" + sceneHeight +
"\nprojRawL[0]=" + projRawL[0] + " projRawL[1]=" + projRawL[1] + " projRawL[2]=" + projRawL[2] + " projRawL[3]=" + projRawL[3] +
"\nprojRawR[0]=" + projRawR[0] + " projRawR[1]=" + projRawR[1] + " projRawR[2]=" + projRawR[2] + " projRawR[3]=" + projRawR[3] +
"\neyes[L]=" + eyes[0].pos + " eyes[R]=" + eyes[1].pos);
configurationChanged = true;
}
private void UpdateHeadPose(float axis_x, float axis_y, float axis_z)
{
if (Input.GetKey(KeyCode.LeftAlt))
{
headAngleX -= axis_y * 2.4f;
headAngleX = Mathf.Clamp(headAngleX, -89, 89);
headAngleY += axis_x * 5;
if (headAngleY <= -180)
{
headAngleY += 360;
}
else if (headAngleY > 180)
{
headAngleY -= 360;
}
}
if (Input.GetKey(KeyCode.LeftControl))
{
headAngleZ += axis_x * 5;
headAngleZ = Mathf.Clamp(headAngleZ, -89, 89);
}
if (Input.GetKey(KeyCode.LeftShift))
{
headPosX += axis_x / 5;
headPosY += axis_y / 5;
headPosZ += axis_z;
}
headPosition.x = headPosX;
headPosition.y = headPosY;
headPosition.z = headPosZ;
if (originModel == WVR_PoseOriginModel.WVR_PoseOriginModel_OriginOnHead_3DoF && enableNeckModel)
{
headPosition = ApplyNeckToHead(headPosition);
}
headPosition.y = originModel == WVR_PoseOriginModel.WVR_PoseOriginModel_OriginOnGround ? headPosition.y + 1.75f : headPosition.y;
headRotation = Quaternion.Euler(headAngleX, headAngleY, headAngleZ);
headRigidTransform.update(headPosition, headRotation);
headPoseMatrix = GetOpenGLMatrix44(headPosition, headRotation);
}
private void UpdateLefHandPose(float axis_x, float axis_y, float axis_z)
{
//-------- mouse control ---------
if (Input.GetKey(KeyCode.C))
{
leftAngleY += axis_x / 2;
leftAngleX -= (float)(axis_y * 1.5f);
}
if (Input.GetKey(KeyCode.X))
{
leftAngleZ += axis_z * 5;
}
leftRotation = Quaternion.Euler(leftAngleX, leftAngleY, leftAngleZ);
if (Input.GetKey(KeyCode.Z))
{
leftPosX += axis_x / 5;
leftPosY += axis_y / 5;
leftPosZ += axis_z;
}
if (simulationType == WVR_SimulationType.WVR_SimulationType_ForceOff ||
(simulationType == WVR_SimulationType.WVR_SimulationType_Auto && is6DoFPose == true)
)
{
leftPosition.x = leftPosX;
leftPosition.y = leftPosY;
leftPosition.z = leftPosZ;
}
if (simulationType == WVR_SimulationType.WVR_SimulationType_ForceOn ||
(simulationType == WVR_SimulationType.WVR_SimulationType_Auto && is6DoFPose == false)
)
{
updateControllerPose(WVR_DeviceType.WVR_DeviceType_Controller_Left, leftRigidTransform);
leftPosition = Vector3.Lerp(leftPosition, controllerSimulatedPosition, smoothMoveSpeed);
}
leftRigidTransform.update(leftPosition, leftRotation);
leftPoseMatrix = GetOpenGLMatrix44(leftPosition, leftRotation);
}
public override void ConvertMatrixQuaternion(ref WVR_Matrix4f_t mat, ref WVR_Quatf_t quat, bool m2q)
{
WVR_ConvertMatrixQuaternion_Android(ref mat, ref quat, m2q);
}
public static extern void WVR_ConvertMatrixQuaternion_Android(ref WVR_Matrix4f_t mat, ref WVR_Quatf_t quat, bool m2q);
public override void ConvertMatrixQuaternion(ref WVR_Matrix4f_t mat, ref WVR_Quatf_t quat, bool m2q)
{
Log.i("WVR_HVR", "ConvertMatrixQuaternion()");
WVR_ConvertMatrixQuaternion_HVR(ref mat, ref quat, m2q);
}
public WVRMatrix(WVR_Matrix4f_t mat)
{
matrix = ToMatrix44(mat);
}
