Merge pull request #22 from LucidVR/feat-21

refactor to include const references instead of pass by value, tidy controller pose, init member variables

LucidGloves/include/Comm/SerialCommunicationManager.h

@@ -22,7 +22,7 @@ struct VRSerialConfiguration_t {
 
 class SerialManager : public ICommunicationManager {
 public:
-	SerialManager(VRSerialConfiguration_t configuration) : m_configuration(configuration) {};
+	SerialManager(VRSerialConfiguration_t configuration) : m_configuration(configuration), m_isConnected(false), m_hSerial(0), m_errors(0) {};
 	//connect to the device using serial
 	void Connect();
 	//start a thread that listens for updates from the device and calls the callback with data
@@ -47,5 +47,4 @@ class SerialManager : public ICommunicationManager {
 	std::thread m_serialThread;
 
 	VRSerialConfiguration_t m_configuration;
-};
-
+};

LucidGloves/include/ControllerDriver.h

@@ -43,7 +43,7 @@ class too. Those comment blocks have some good information.
 **/
 class ControllerDriver : public vr::ITrackedDeviceServerDriver {
 public:
-	ControllerDriver(const VRDeviceConfiguration_t settings);
+	ControllerDriver(const VRDeviceConfiguration_t &settings);
 	/**
 	Initialize your controller here. Give OpenVR information
 	about your controller and set up handles to inform OpenVR when

LucidGloves/include/ControllerPose.h

@@ -17,9 +17,6 @@ class ControllerPose {
 	//This member variable is the id of the controller we are "shadowing" and receiving positioning from.
 	short int m_shadowControllerId = -1;
 
-	//This is the pose we send back for OpenVR to process as our pose for the driver.
-	//This represents the orientation and rotation, as well as angular velocity, etc.
-	vr::DriverPose_t m_pose;
 	vr::HmdQuaternion_t m_offsetQuaternion;
 
 	VRDeviceConfiguration_t m_configuration;

LucidGloves/include/Quaternion.h

@@ -8,13 +8,14 @@ double DegToRad(int degrees);
 double RadToDeg(double rad);
 
 //get the quaternion for roation from a matrix
-vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t matrix);
-vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t matrix);
+vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t& matrix);
+vr::HmdVector3_t GetPosition(const vr::HmdMatrix34_t& matrix);
+vr::HmdVector3_t CombinePosition(const vr::HmdMatrix34_t& matrix, const vr::HmdVector3_t& vec);
 
 //returns the result of multiplying two quaternions, effectively applying a roatation on a quaternion
-vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t q, const vr::HmdQuaternion_t r);
+vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t& q, const vr::HmdQuaternion_t& r);
 
 vr::HmdQuaternion_t QuaternionFromAngle(const double& xx, const double& yy, const double& zz, const double& a);
 vr::HmdQuaternion_t EulerToQuaternion(const double& x, const double& y, const double& z);
-vr::HmdMatrix33_t Get33Matrix(const vr::HmdMatrix34_t matrix);
-vr::HmdVector3_t MultiplyMatrix(vr::HmdMatrix33_t matrix, const vr::HmdVector3_t vector);
+vr::HmdMatrix33_t GetRotationMatrix(const vr::HmdMatrix34_t& matrix);
+vr::HmdVector3_t MultiplyMatrix(const vr::HmdMatrix33_t& matrix, const vr::HmdVector3_t& vector);

LucidGloves/src/ControllerDriver.cpp

@@ -1,7 +1,7 @@
 #include "ControllerDriver.h"
 
-ControllerDriver::ControllerDriver(const VRDeviceConfiguration_t configuration)
-	: m_configuration(configuration) {
+ControllerDriver::ControllerDriver(const VRDeviceConfiguration_t &configuration)
+	: m_configuration(configuration), m_driverId(-1) {
 
 	//copy a default bone transform to our hand transform for use in finger positioning later
 	std::copy(

LucidGloves/src/ControllerPose.cpp

@@ -2,43 +2,43 @@
 #include "ControllerPose.h"
 
 ControllerPose::ControllerPose(vr::ETrackedControllerRole shadowDeviceOfRole, std::string thisDeviceManufacturer, VRDeviceConfiguration_t configuration, uint32_t driverId) : m_configuration(configuration), m_shadowDeviceOfRole(shadowDeviceOfRole), m_driverId(driverId), m_thisDeviceManufacturer(thisDeviceManufacturer) {
-
-	m_pose.poseIsValid = true;
-	m_pose.result = vr::TrackingResult_Running_OK;
-	m_pose.deviceIsConnected = true;
-	m_pose.qWorldFromDriverRotation.w = 1;
-	m_pose.qDriverFromHeadRotation.w = 1;
-	//This will be configurable in settings
-	m_pose.poseTimeOffset = configuration.poseOffset;
 	const vr::HmdVector3_t angleOffset = m_configuration.angleOffsetVector;
 	m_offsetQuaternion = EulerToQuaternion(DegToRad(angleOffset.v[0]), DegToRad(angleOffset.v[1]), DegToRad(angleOffset.v[2]));
+
 	DebugDriverLog("Offset calculated! {%.2f, %.2f, %.2f, %.2f}", m_offsetQuaternion.w, m_offsetQuaternion.x, m_offsetQuaternion.y, m_offsetQuaternion.z);
 }
 
 vr::DriverPose_t ControllerPose::UpdatePose() {
 	vr::DriverPose_t newPose = { 0 };
+	newPose.qWorldFromDriverRotation.w = 1;
+	newPose.qDriverFromHeadRotation.w = 1;
+
 	if (m_shadowControllerId != -1) {
 		vr::TrackedDevicePose_t trackedDevicePoses[vr::k_unMaxTrackedDeviceCount];
 		vr::VRServerDriverHost()->GetRawTrackedDevicePoses(0, trackedDevicePoses, vr::k_unMaxTrackedDeviceCount);
 
 		if (trackedDevicePoses[m_shadowControllerId].bPoseIsValid) {
-			newPose.qWorldFromDriverRotation.w = 1;
-			newPose.qDriverFromHeadRotation.w = 1;
 
-			const vr::HmdMatrix34_t shadowControllerMatrix = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking;
-
-			////As we need to account for rotation for the offset, multiply 
-			const vr::HmdVector3_t vectorOffset = MultiplyMatrix(Get33Matrix(shadowControllerMatrix), m_configuration.offsetVector);
+			//get the matrix that represents the position of the controller that we are shadowing
+			vr::HmdMatrix34_t controllerMatrix = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking;
+
+			//get only the rotation (3x3 matrix), as the 3x4 matrix also includes position
+			vr::HmdMatrix33_t controllerRotationMatrix = GetRotationMatrix(controllerMatrix);
+
+			//multiply the rotation matrix by the offset vector set that is the offset of the controller relative to the hand
+			vr::HmdVector3_t vectorOffset = MultiplyMatrix(controllerRotationMatrix, m_configuration.offsetVector); 
+
+			//combine these positions to get the resultant position
+			vr::HmdVector3_t newControllerPosition = CombinePosition(controllerMatrix, vectorOffset);
 
-			newPose.vecPosition[0] = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking.m[0][3] + vectorOffset.v[0];
-			newPose.vecPosition[1] = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking.m[1][3] + vectorOffset.v[1];
-			newPose.vecPosition[2] = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking.m[2][3] + vectorOffset.v[2]; //- forward
+			newPose.vecPosition[0] = newControllerPosition.v[0];
+			newPose.vecPosition[1] = newControllerPosition.v[1];
+			newPose.vecPosition[2] = newControllerPosition.v[2];
 
-			//vr::HmdQuaternion_t offset_quaternion = QuaternionFromAngle(1, 0, 0, DegToRad(-45));
-
-			//merge rotation
-			newPose.qRotation = MultiplyQuaternion(GetRotation(shadowControllerMatrix), m_offsetQuaternion);
+			//Multiply rotation quaternions together, as the controller may be rotated relative to the hand
+			newPose.qRotation = MultiplyQuaternion(GetRotation(controllerMatrix), m_offsetQuaternion);
 
+			//Copy other values from the controller that we want for this device
 			newPose.vecAngularVelocity[0] = trackedDevicePoses[m_shadowControllerId].vAngularVelocity.v[0];
 			newPose.vecAngularVelocity[1] = trackedDevicePoses[m_shadowControllerId].vAngularVelocity.v[1];
 			newPose.vecAngularVelocity[2] = trackedDevicePoses[m_shadowControllerId].vAngularVelocity.v[2];
@@ -49,11 +49,11 @@ vr::DriverPose_t ControllerPose::UpdatePose() {
 
 			newPose.poseIsValid = true;
 			newPose.deviceIsConnected = true;
+
 			newPose.result = vr::TrackingResult_Running_OK;
 
 		}
 		else {
-			//DebugDriverLog("pose %d is not valid", m_shadowControllerId);
 			newPose.poseIsValid = false;
 			newPose.deviceIsConnected = true;
 			newPose.result = vr::TrackingResult_Uninitialized;

LucidGloves/src/DeviceFactory.cpp

@@ -20,7 +20,7 @@ This method returns an instance of your provider that OpenVR uses.
 HMD_DLL_EXPORT
 void* HmdDriverFactory(const char* interfaceName, int* returnCode) {
 
-	if (0 == strcmp(vr::IServerTrackedDeviceProvider_Version, interfaceName)) 	{
+	if (0 == strcmp(vr::IServerTrackedDeviceProvider_Version, interfaceName)) {
 		return &deviceProvider;
 	}
 	DriverLog("HmdDriverFactory called for %s", interfaceName);

LucidGloves/src/Quaternion.cpp

@@ -7,7 +7,7 @@ double RadToDeg(double rad) {
 	return rad * 180 / M_PI;
 }
 
-static vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t matrix) {
+vr::HmdVector3_t GetPosition(const vr::HmdMatrix34_t& matrix) {
 	vr::HmdVector3_t vector;
 
 	vector.v[0] = matrix.m[0][3];
@@ -17,7 +17,17 @@ static vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t matrix) {
 	return vector;
 }
 
-vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t matrix) {
+vr::HmdVector3_t CombinePosition(const vr::HmdMatrix34_t& matrix, const vr::HmdVector3_t& vec) {
+	vr::HmdVector3_t vector;
+
+	vector.v[0] = matrix.m[0][3] + vec.v[0];
+	vector.v[1] = matrix.m[1][3] + vec.v[1];
+	vector.v[2] = matrix.m[2][3] + vec.v[2];
+
+	return vector;
+}
+
+vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t& matrix) {
 	vr::HmdQuaternion_t q;
 
 	q.w = sqrt(fmax(0, 1 + matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2])) / 2;
@@ -32,7 +42,7 @@ vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t matrix) {
 	return q;
 }
 
-vr::HmdMatrix33_t Get33Matrix(const vr::HmdMatrix34_t matrix) {
+vr::HmdMatrix33_t GetRotationMatrix(const vr::HmdMatrix34_t& matrix) {
 	vr::HmdMatrix33_t result = { {
 		{matrix.m[0][0], matrix.m[0][1], matrix.m[0][2]},
 		{matrix.m[1][0], matrix.m[1][1], matrix.m[1][2]},
@@ -41,7 +51,7 @@ vr::HmdMatrix33_t Get33Matrix(const vr::HmdMatrix34_t matrix) {
 
 	return result;
 }
-vr::HmdVector3_t MultiplyMatrix(const vr::HmdMatrix33_t matrix, const vr::HmdVector3_t vector) {
+vr::HmdVector3_t MultiplyMatrix(const vr::HmdMatrix33_t& matrix, const vr::HmdVector3_t& vector) {
 	vr::HmdVector3_t result;
 
 	result.v[0] = matrix.m[0][0] * vector.v[0] + matrix.m[0][1] * vector.v[1] + matrix.m[0][2] * vector.v[2];
@@ -72,15 +82,14 @@ vr::HmdQuaternion_t QuaternionFromAngle(const double& xx, const double& yy, cons
 	return quat;
 }
 
-vr::HmdQuaternion_t EulerToQuaternion(const double& x, const double& y, const double& z)
-{
+vr::HmdQuaternion_t EulerToQuaternion(const double& x, const double& y, const double& z) {
 	vr::HmdQuaternion_t quat0 = QuaternionFromAngle(1, 0, 0, x);
 	vr::HmdQuaternion_t quat1 = MultiplyQuaternion(quat0, QuaternionFromAngle(0, 1, 0, y));
 	vr::HmdQuaternion_t quat2 = MultiplyQuaternion(quat1, QuaternionFromAngle(0, 0, 1, z));
 	return quat2;
 }
 
-vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t q, const vr::HmdQuaternion_t r) {
+vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t& q, const vr::HmdQuaternion_t& r) {
 	vr::HmdQuaternion_t result;
 
 	result.w = (r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z);