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>");
}
public bool getFramePose(ref WVR_PoseState_t pose)
{
if (!syncPose)
{
return(false);
}
pose = mPoseState;
return(true);
}
public void startCamera(bool enable)
{
if (mStarted)
{
return;
}
syncPose = enable;
WaveVR_Utils.Event.Listen("DrawCameraCompleted", OnUpdateCameraCompleted);
if (syncPose)
{
mPoseState = new WVR_PoseState_t();
mStarted = Interop.WVR_StartCamera(ref camerainfo);
Log.i(LOG_TAG, "startCamera, result = " + mStarted + " format: " + camerainfo.imgFormat + " size: " + camerainfo.size
+ " width: " + camerainfo.width + " height: " + camerainfo.height);
PrintDebugLog("allocate frame buffer");
mframeBuffer = Marshal.AllocHGlobal((int)camerainfo.size);
//zero out buffer
for (int i = 0; i < camerainfo.size; i++)
{
Marshal.WriteByte(mframeBuffer, i, 0);
}
if (StartCameraCompletedDelegate != null)
{
StartCameraCompletedDelegate(mStarted);
}
}
else
{
mthread = new Thread(() => CameraThread());
if (mthread.IsBackground == false)
{
mthread.IsBackground = true;
}
toThreadStop = false;
mthread.Start();
}
}
public override void GetPoseState(WVR_DeviceType type, WVR_PoseOriginModel originModel, uint predictedMilliSec, ref WVR_PoseState_t poseState)
{
WVR_GetPoseState_Android(type, originModel, predictedMilliSec, ref poseState);
}
public static extern void WVR_GetPoseState_Android(WVR_DeviceType type, WVR_PoseOriginModel originModel, uint predictedMilliSec, ref WVR_PoseState_t poseState);
private void DetectPoseLoop()
{
float fDegreesDiff = 0.0f;
// WVR_PoseState_t LastPose;
// WVR_PoseState_t NowPose;
long tNsNowGet = 0;
long tNsGenPose = 0;
long tNsFromGenToGet = 0;
long tMsFromGenToGet = 0;
long tNsLastTimeGet = 0;
long tNsLastTimeGen = 0;
long tNsFromLastGenToNowGet = 0;
long tMsFromLastGenToNowGet = 0;
long tNsJudgePose = 0;
long tMsJudgePose = 0;
bool bConvertMatrixToQuaternion = false;
bool needPrintTimeLog = false;
offsetTimeDSPToSystem = 0; //gQTimeToAndroidBoot
bIsRunningThread = true;
while (!bExitDetectPoseLoop)
{
Interop.WVR_GetPoseState(g_typeDevice, WVR_PoseOriginModel.WVR_PoseOriginModel_OriginOnTrackingObserver, 0, ref NowPose);
// Interop.WVR_GetPoseState (g_typeDevice,WVR_PoseOriginModel.WVR_PoseOriginModel_OriginOnHead_3DoF,0,ref NowPose);
if (NowPose.IsValidPose)
{
if (tNsGenPose == NowPose.PoseTimestamp_ns) //得到上次有效的同样的数据
{
Thread.Sleep(1000 / ReadPoseFrequence);
continue;
}
//tNsJudgePose = il2cpp.icalls.mscorlib.System.DateTime.GetTimeMonotonic();
//tNsJudgePose = GetTimeMonotonic();
//tNsJudgePose = System.DateTime.Now.Millisecond * 1000000l;
tNsJudgePose = GetNanoTime();
//tNsJudgePose *= 10^6;
if (FLAG_DEBUG)
{
DumpPoseState(NowPose);
}
// if(!m_isHmd && 0 == NowPose.RawPose.rotation.w &&
if (0 == NowPose.RawPose.rotation.w &&
0 == NowPose.RawPose.rotation.x &&
0 == NowPose.RawPose.rotation.y &&
0 == NowPose.RawPose.rotation.z) //use RawPose// Controller not get RawPose;
{
bConvertMatrixToQuaternion = true;
Interop.WVR_ConvertMatrixQuaternion(ref NowPose.PoseMatrix, ref NowPose.RawPose.rotation, true); //WVR_Quatf_t
if (FLAG_DEBUG)
{
Log.d(LOG_TAG, "Convert Matrix To Quaternion!!!");
}
if (FLAG_DEBUG)
{
DumpPoseState(NowPose);
}
}
currentQuaternion.Set(NowPose.RawPose.rotation.x, NowPose.RawPose.rotation.y, NowPose.RawPose.rotation.z, NowPose.RawPose.rotation.w);
// currentQuaternion.w = NowPose.RawPose.rotation.w;
// currentQuaternion.x = NowPose.RawPose.rotation.x;
// currentQuaternion.y = NowPose.RawPose.rotation.y;
// currentQuaternion.z = NowPose.RawPose.rotation.z;
fDegreesDiff = Quaternion.Angle(preQuaternion, currentQuaternion);
if (NowPose.Is6DoFPose)
{
currentPosition.x = NowPose.RawPose.position.v0;
currentPosition.y = NowPose.RawPose.position.v1;
currentPosition.z = NowPose.RawPose.position.v2;
detaPosition = currentPosition - prePosition;
}
if ((Mathf.Abs(fDegreesDiff) > gMinDegreeMoving) || (
detaPosition.x > gPositionThresholdMotionToPhoton ||
detaPosition.y > gPositionThresholdMotionToPhoton ||
detaPosition.z > gPositionThresholdMotionToPhoton
))
{
// openLightLED(true);
changeColor();
needPrintTimeLog = true;
IsMovingByPoseData = true;
nLastHightLightFrameCount = Time.frameCount;
Log.d(LOG_TAG, "High Light LED and LCD BackLight!!fDegreesDiff=" + fDegreesDiff + ",bConvertMatrixToQuaternion=" + bConvertMatrixToQuaternion + ".");
/*Log.d (LOG_TAG, "Delay Time:: offsetTimeDSPToSystem="+offsetTimeDSPToSystem+
* ",tNsNowGet="+tNsNowGet+",tNsGenPose="+tNsGenPose+
* ",tNsFromGenToGet="+tNsFromGenToGet+",tNsJudgePose="+tNsJudgePose+
* ",tNsFromLastGenToNowGet="+tNsFromLastGenToNowGet+
* ",tMsFromGenToGet="+tMsFromGenToGet+
* ",tMsFromLastGenToNowGet="+tMsFromLastGenToNowGet+".");*/
if (Mathf.Abs(fDegreesDiff) <= gMinDegreeMoving)
{
Log.d(LOG_TAG, "detaPosition is ={" + detaPosition.x + "," + detaPosition.y + "," + detaPosition.z + "}.");
}
DumpQuatertion(preQuaternion);
DumpQuatertion(currentQuaternion);
}
else
{
if (Time.frameCount > nLastHightLightFrameCount + 2) //确保至少亮一帧以上时间以便捕捉 再延长1帧
// openLightLED(false);
{
clearColor();
}
IsMovingByPoseData = false;
}
if (FLAG_DEBUG)
{
Log.d(LOG_TAG, "fDegreesDiff=" + fDegreesDiff + ",preQuaternion+currentQuaternion is ");
DumpQuatertion(preQuaternion);
DumpQuatertion(currentQuaternion);
}
preQuaternion = currentQuaternion;
prePosition = currentPosition;
LastPose = NowPose;
if (m_isHmd && isFixSvrHmdposeTimeStamp)
{
if (0 == tNsLastTimeGen)
{
if (tNsGenPose > tNsNowGet)
{
offsetTimeDSPToSystem = tNsNowGet - tNsGenPose - (SET_MS_FromGetToGen * 1000000); //tNsFromGenToGet = tNsNowGet - tNsGenPose;
}
}
}
//tNsNowGet = GetTimeMonotonic();
//tNsNowGet = System.DateTime.Now.Millisecond * 1000000l;
tNsNowGet = GetNanoTime();
tNsJudgePose = tNsNowGet - tNsJudgePose;
tMsJudgePose = tNsJudgePose / 1000000;
if (m_isHmd && isFixSvrHmdposeTimeStamp)
{
tNsFromLastGenToNowGet = tNsNowGet - (tNsLastTimeGen + offsetTimeDSPToSystem); //nTime from LastPoseGenTimeNs to NowGetDataTimeNs
}
else
{
tNsFromLastGenToNowGet = tNsNowGet - tNsLastTimeGen; //nTime from LastPoseGenTimeNs to NowGetDataTimeNs
}
tMsFromLastGenToNowGet = tNsFromLastGenToNowGet / 1000000; //nTime from LastPoseGenTimeNs to NowGetDataTimeNs
tNsGenPose = NowPose.PoseTimestamp_ns;
if (m_isHmd && isFixSvrHmdposeTimeStamp)
{
tNsFromGenToGet = tNsNowGet - (tNsGenPose + offsetTimeDSPToSystem);
}
else
{
tNsFromGenToGet = tNsNowGet - tNsGenPose;
}
tMsFromGenToGet = tNsFromGenToGet / 1000000;
tNsLastTimeGen = tNsGenPose;
tNsLastTimeGet = tNsNowGet;
if (needPrintTimeLog)
{
Log.d(LOG_TAG, "Delay Time:: offsetTimeDSPToSystem=" + offsetTimeDSPToSystem +
",tNsNowGet=" + tNsNowGet + ",tNsGenPose=" + tNsGenPose +
",tNsFromGenToGet=" + tNsFromGenToGet + ",tNsJudgePose=" + tNsJudgePose +
",tNsFromLastGenToNowGet=" + tNsFromLastGenToNowGet +
",tMsFromGenToGet=" + tMsFromGenToGet +
",tMsFromLastGenToNowGet=" + tMsFromLastGenToNowGet + ".");
}
preRawQuaternion = currentRawQuaternion;
if (tMsFromGenToGet - tMsJudgePose > 10)
{
Log.d(LOG_TAG, "Get Time is long!!tMsFromGenToGet=" + tMsFromGenToGet + ",tMsJudgePose=" + tMsJudgePose + ";");
}
if (tMsJudgePose > 10)
{
Log.d(LOG_TAG, "Judge Pose Time is long!!tNsJudgePose=" + tMsJudgePose + ";");
}
needPrintTimeLog = false;
Thread.Sleep(1000 / ReadPoseFrequence);
}
}
}
