Skip to content

Commit

Permalink
Merge pull request #1 from Dschadu/beta-0.1.0
Browse files Browse the repository at this point in the history 
Beta 0.1.0
  • Loading branch information
@Dschadu
Dschadu authored Aug 8, 2020
2 parents ffc3ec4 + fd9363e commit e5d0a8c
Show file tree
Hide file tree
Showing 3 changed files with 202 additions and 167 deletions.
262 changes: 126 additions & 136 deletions motionPose/MotionPoseController.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,49 +1,45 @@
#include "MotionPoseController.h"
#include "driverlog.h"
#include "third-party/easylogging++/easylogging++.h"

INITIALIZE_EASYLOGGINGPP

//class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver
namespace driver
{
CMotionPoseControllerDriver::CMotionPoseControllerDriver()
{
m_unObjectId = vr::k_unTrackedDeviceIndexInvalid;
m_ulPropertyContainer = vr::k_ulInvalidPropertyContainer;

m_sSerialNumber = "MotionPoseVirtualController 0.1";

m_sSerialNumber = "MotionPoseVirtualController 0.2.0";
m_sModelNumber = "MotionPoseVirtualController";
rigPitch = 0;
rigRoll = 0;
rigYaw = 0;
rigHeave = 0;
rigSurge = 0;
rigSway = 0;
yawComp = 0;
pose = { 0 };
pose.qWorldFromDriverRotation = HmdQuaternion_Init(1, 0, 0, 0);
pose.qDriverFromHeadRotation = HmdQuaternion_Init(1, 0, 0, 0);
pose.qRotation = HmdQuaternion_Init(1, 0, 0, 0);
pose.vecDriverFromHeadTranslation[0] = 0;
pose.vecDriverFromHeadTranslation[1] = 0;
pose.vecDriverFromHeadTranslation[2] = 0;
pose.vecWorldFromDriverTranslation[0] = 0;
pose.vecWorldFromDriverTranslation[1] = 0;
pose.vecWorldFromDriverTranslation[2] = 0;
pose.vecPosition[0] = 0;
pose.vecPosition[1] = 0;
pose.vecPosition[2] = 0;
rigPos = { 0 };

rigYawOffset = 0;

_pose.poseTimeOffset = 0;
_pose.qWorldFromDriverRotation = { 1, 0, 0, 0 };
_pose.qDriverFromHeadRotation = { 1, 0, 0, 0 };
_pose.qRotation = { 1, 0, 0, 0 };
_pose.vecDriverFromHeadTranslation[0] = 0;
_pose.vecDriverFromHeadTranslation[1] = 0;
_pose.vecDriverFromHeadTranslation[2] = 0;
_pose.vecWorldFromDriverTranslation[0] = 0;
_pose.vecWorldFromDriverTranslation[1] = 0;
_pose.vecWorldFromDriverTranslation[2] = 0;
_pose.vecPosition[0] = 0;
_pose.vecPosition[1] = 0;
_pose.vecPosition[2] = 0;

rigOffset = { 0, 0, 0 };
}

CMotionPoseControllerDriver::~CMotionPoseControllerDriver()
{
}

#define BUF_SIZE 256

vr::EVRInitError CMotionPoseControllerDriver::Activate(vr::TrackedDeviceIndex_t unObjectId)
{
DriverLog("Activate motionPoseController\n");
init_logging();
LOG(INFO) << "Activate motionPoseController\n";

m_unObjectId = unObjectId;
m_ulPropertyContainer = vr::VRProperties()->TrackedDeviceToPropertyContainer(m_unObjectId);
Expand All @@ -60,38 +56,43 @@ namespace driver
return vr::VRInitError_None;
}

void CMotionPoseControllerDriver::initalizeMoverMmf()
void CMotionPoseControllerDriver::init_logging()
{
moverConnected = false;
TCHAR szName[] = TEXT("Local\\motionRigPose");
el::Loggers::addFlag(el::LoggingFlag::DisableApplicationAbortOnFatalLog);
el::Configurations conf(logConfigFileName);
conf.parseFromText(logConfigDefault);
//conf.parseFromFile(logConfigFileName);
conf.setRemainingToDefault();
el::Loggers::reconfigureAllLoggers(conf);

LOG(INFO) << "|========================================================================================|";
LOG(INFO) << "motionPose dll loaded...";
LOG(TRACE) << "Trace messages enabled.";
LOG(DEBUG) << "Debug messages enabled.";
}

hMapFile = OpenFileMapping(
FILE_MAP_ALL_ACCESS, // read/write access
FALSE, // do not inherit the name
szName); // name of mapping object
bool CMotionPoseControllerDriver::openMmf(HANDLE& MapFile, char*& mmfFile, LPCWSTR szName, int BufferSize, bool& Connected)
{
MapFile = OpenFileMapping(FILE_MAP_ALL_ACCESS, FALSE, szName);

if (hMapFile == NULL)
// return if MMF could not be opened
if (MapFile == NULL)
{
// DriverLog("could not open mmf motionRigPose\n");
return false;
}
else

mmfFile = (char*)MapViewOfFile(MapFile, FILE_MAP_READ, 0, 0, BufferSize);

// return if view could not be mapped
if (mmfFile == NULL)
{
// DriverLog("successfully opened mmf motionRigPose\n");
mmfFile = (char*)MapViewOfFile(hMapFile, // handle to map object
FILE_MAP_ALL_ACCESS, // read/write permission
0,
0,
BUF_SIZE);
if (strncmp(mmfFile, "posedata", 8) == 0)
{
DriverLog("successfully receiving posedata\n");
moverConnected = true;
}
else
{
// DriverLog("no valid posedata\n");
}
CloseHandle(MapFile);
return false;
}

Connected = true;

return true;
}

void CMotionPoseControllerDriver::Deactivate()
Expand Down Expand Up @@ -122,106 +123,95 @@ namespace driver

vr::DriverPose_t CMotionPoseControllerDriver::GetPose()
{
// DriverLog("virtualPoseController::GetPose\n");

pose.poseIsValid = true;
pose.deviceIsConnected = true;
pose.shouldApplyHeadModel = false;
pose.willDriftInYaw = false;
_pose.poseIsValid = false;
_pose.deviceIsConnected = _moverConnected;
_pose.shouldApplyHeadModel = false;
_pose.willDriftInYaw = false;

vr::HmdQuaternion_t yaw = vrmath::quaternionFromRotationY(yawComp);
vr::HmdQuaternion_t yaw = vrmath::quaternionFromRotationY(rigYawOffset);

// Turn Right/Left
if ((GetAsyncKeyState('E') & 0x8000) != 0) yawComp += 0.005;
if ((GetAsyncKeyState('R') & 0x8000) != 0) yawComp -= 0.005;

// Offset Left/Right
if ((GetAsyncKeyState('Q') & 0x8000) != 0) rigPos = rigPos + vrmath::quaternionRotateVector(yaw, HmdVector3d_t_Init(-0.003, 0, 0));
if ((GetAsyncKeyState('W') & 0x8000) != 0) rigPos = rigPos + vrmath::quaternionRotateVector(yaw, HmdVector3d_t_Init(0.003, 0, 0));

// Offset Up/Down
if ((GetAsyncKeyState('A') & 0x8000) != 0) rigPos = rigPos + vrmath::quaternionRotateVector(yaw, HmdVector3d_t_Init(0, 0.003, 0));
if ((GetAsyncKeyState('S') & 0x8000) != 0) rigPos = rigPos + vrmath::quaternionRotateVector(yaw, HmdVector3d_t_Init(0, -0.003, 0));

// Offset Forward/Backward
if ((GetAsyncKeyState('Y') & 0x8000) != 0) rigPos = rigPos + vrmath::quaternionRotateVector(yaw, HmdVector3d_t_Init(0, 0, -0.003));
if ((GetAsyncKeyState('X') & 0x8000) != 0) rigPos = rigPos + vrmath::quaternionRotateVector(yaw, HmdVector3d_t_Init(0, 0, 0.003));

// reset
if ((GetAsyncKeyState('P') & 0x8000) != 0)
// Create connection to OVRMC
if (!_ovrmcConnected)
{
rigPos = { 0 };
yawComp = 0;
if (openMmf(MapFile_OVRMC, mmfFile_OVRMC, L"Local\\OVRMC_MMFv1", 4096, _ovrmcConnected))
{
Data_OVRMC = (MMFstruct_OVRMC_v1*)mmfFile_OVRMC;
}
}

if (moverConnected && strncmp(mmfFile, "posedata", 8) == 0)
else
{
pose.result = vr::TrackingResult_Running_OK;
// <PoseSway><PoseSurge><PoseHeave><PoseYaw><PoseRoll><PosePitch>
double* poseData = (double*)mmfFile;
rigSway = poseData[1];
rigSurge = poseData[2];
rigHeave = poseData[3];
rigYaw = poseData[4];
rigRoll = poseData[5];
rigPitch = poseData[6];
/* char buf[100];
sprintf_s(buf, "virtualPoseController::rigYaw: %lf\n", rigYaw);
DriverLog(buf);
sprintf_s(buf, "virtualPoseController::rigPitch: %lf\n", rigPitch);
DriverLog(buf);*/

vr::HmdVector3d_t rigTranslation = { 0 };
rigTranslation.v[0] = rigSway / 1000;
rigTranslation.v[1] = rigHeave / 1000;
rigTranslation.v[2] = -rigSurge / 1000;

pose.qRotation = vrmath::quaternionFromYawRollPitch(yawComp + rigYaw * 0.01745329251994329, -rigRoll * 0.01745329251994329, rigPitch * 0.01745329251994329);
vr::HmdVector3d_t rigVector = vrmath::quaternionRotateVector(pose.qRotation, rigTranslation) + rigPos;
pose.vecPosition[0] = rigVector.v[0];
pose.vecPosition[1] = rigVector.v[1];
pose.vecPosition[2] = rigVector.v[2];

/* pose.qWorldFromDriverRotation = vrmath::quaternionFromYawPitchRoll(yawComp, 0, 0);
pose.vecWorldFromDriverTranslation[0] = rigX;
pose.vecWorldFromDriverTranslation[1] = rigY;
pose.vecWorldFromDriverTranslation[2] = rigZ;*/

return pose;
// Get data from OVRMC
rigOffset = Data_OVRMC->Translation;
rigYawOffset = Data_OVRMC->Rotation.v[1];
}
else

if (!_moverConnected)
{
pose.result = vr::TrackingResult_Calibrating_InProgress;
pose.deviceIsConnected = false;
initalizeMoverMmf();
_pose.result = vr::TrackingResult_Calibrating_InProgress;
_pose.qRotation = vrmath::quaternionFromYawRollPitch(rigYawOffset, 0, 0);

pose.qRotation = vrmath::quaternionFromYawRollPitch(yawComp, 0, 0);
vr::HmdVector3d_t rigVector = rigPos;
// Init was unsuccessful, return empty pose
if (!openMmf(MapFile_Mover, mmfFile_Mover, L"Local\\motionRigPose", 4096, _moverConnected))
{
return _pose;
}

return pose;
rigPose = (MMFstruct_Mover_v1*)mmfFile_Mover;
}


// Convert from mm to m
vr::HmdVector3d_t rigTranslation = { 0 };
rigTranslation.v[0] = rigPose->rigSway / (double)1000.0;
rigTranslation.v[1] = rigPose->rigHeave / (double)1000.0;
rigTranslation.v[2] = -rigPose->rigSurge / (double)1000.0;

// Create the rotation. Convert from degree to radian
_pose.qRotation = vrmath::quaternionFromYawRollPitch(rigYawOffset + rigPose->rigYaw * 0.01745329251994329, -rigPose->rigRoll * 0.01745329251994329, rigPose->rigPitch * 0.01745329251994329);

// Create the translation (XYZ position in 3d space)
vr::HmdVector3d_t rigVector = vrmath::quaternionRotateVector(_pose.qRotation, rigTranslation) + rigOffset;
_pose.vecPosition[0] = rigVector.v[0];
_pose.vecPosition[1] = rigVector.v[1];
_pose.vecPosition[2] = rigVector.v[2];

_pose.poseIsValid = true;
_pose.result = vr::TrackingResult_Running_OK;

return _pose;
}

void CMotionPoseControllerDriver::RunFrame()
{
// Your driver would read whatever hardware state is associated with its input components and pass that
// in to UpdateBooleanComponent. This could happen in RunFrame or on a thread of your own that's reading USB
// state. There's no need to update input state unless it changes, but it doesn't do any harm to do so.
// DriverLog(" virtualPoseController::RunFrame\n");

// Collect events
/* vr::VREvent_t event;
while (vr::VRServerDriverHost()->PollNextEvent(&event, sizeof(event)))
try
{
if (event.trackedDeviceIndex == this->m_unObjectId && event.eventType == EVREventType::VREvent_PropertyChanged) {
if (event.data.property.prop == Prop_RenderModelName_String) {
std::string renderModel = vr::VRProperties()->GetStringProperty(event.data.property.container, Prop_RenderModelName_String);
vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, Prop_RenderModelName_String, renderModel.data());
}
GetPose();
}
catch (std::exception& e)
{
LOG(ERROR) << "MMF failed: " << e.what();

// Close mover connection
if (CloseHandle(MapFile_Mover) == 0)
{
LOG(ERROR) << "Failed to close Mover handle! Error: " << GetLastError();
}

// Close ovrmc connection
if (CloseHandle(MapFile_OVRMC) == 0)
{
LOG(ERROR) << "Failed to close OVRMC handle! Error: " << GetLastError();
}
}*/

vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_unObjectId, GetPose(), sizeof(vr::DriverPose_t));
_moverConnected = false;
_ovrmcConnected = false;

// Declare pose as invalid
_pose.poseIsValid = false;
_pose.result = vr::TrackingResult_Calibrating_InProgress;
}

vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_unObjectId, _pose, sizeof(vr::DriverPose_t));

}
};
Loading

0 comments on commit e5d0a8c

Please sign in to comment.