/*
* Oculus Rift - Lots of obsolete functions, bits of which might be needed in the future if Oculus Unity Utilities support will be added
*/
// Oculus positional tracking camera's coordinate system origin in Unity coordinate system
// public Vector3 GetOculusCameraOriginRaw()
// {
//// if (OVRManager.capiHmd != null)
//// {
//// Vector3 currentOvrCameraPose = OVRManager.capiHmd.GetTrackingState().CameraPose.Position.ToVector3 (); //06to08
////
//// return Quaternion.Inverse(GetOculusCameraOrientationRaw())*currentOvrCameraPose;
//// } else //06to08
// return Vector3.zero; // HACK remove this method
// }
// Oculus positional tracking camera's coordinate system origin in master coordinate system
// public Vector3 GetOculusCameraOrigin()
// {
// return ConvertLocation(GetOculusCameraOriginRaw(), RUISDevice.Oculus_DK2);
// }
// Oculus positional tracking camera's coordinate system orientation in Unity coordinates
// public Quaternion GetOculusCameraOrientationRaw()
// {
//// if (OVRManager.capiHmd != null)
//// {
//// return OVRManager.capiHmd.GetTrackingState().CameraPose.Orientation.ToQuaternion(); //06to08
//// } else
// return Quaternion.identity; // HACK remove this method
// }
// public Vector3 ConvertRawOculusDK2Location(Vector3 position)
// {
//// Vector3 currentcameraPosition = Vector3.zero;
//// if (OVRManager.capiHmd != null)
//// currentcameraPosition = OVRManager.capiHmd.GetTrackingState().CameraPose.Position.ToVector3(); //06to08
//// return Quaternion.Inverse(GetOculusCameraOrientationRaw())*(position - currentcameraPosition); //06to08
//
// return UnityEngine.VR.InputTracking.GetLocalPosition(UnityEngine.VR.VRNode.Head); // HACK TODO if this doesn't work for major HMDs, add wrapper
// }
/// <summary>
/// Convert velocity or angular velocity obtained with a certain device to master coordinate system, apply yaw offset, and apply Kinect pitch correction
/// </summary>
public Vector3 ConvertVelocity(Vector3 velocity, RUISDevice device)
{
Vector3 newVelocity = velocity;
if (applyToRootCoordinates && rootDevice != device)
{
string devicePairString = device.ToString() + "-" + rootDevice.ToString();
newVelocity = RUISCalibrationResultsInQuaternion[devicePairString] * newVelocity;
}
// Apply floor pitch rotation (which is identity to anything else than Kinect 1/2)
if (applyToRootCoordinates || device == rootDevice)
{
newVelocity = RUISCalibrationResultsFloorPitchRotation[rootDevice] * newVelocity;
}
else
{
if (device == RUISDevice.Kinect_2)
{
newVelocity = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_2] * newVelocity;
}
else if (device == RUISDevice.Kinect_1)
{
newVelocity = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_1] * newVelocity;
}
}
newVelocity = Quaternion.Euler(0, yawOffset, 0) * newVelocity;
return(newVelocity);
}
/// <summary>
/// Convert rotation obtained with a certain device to master coordinate system, apply yaw offset, and apply Kinect pitch correction
/// </summary>
public Quaternion ConvertRotation(Quaternion inputRotation, RUISDevice device)
{
Quaternion outputRotation = inputRotation;
if (applyToRootCoordinates && rootDevice != device)
{
string devicePairString = device.ToString() + "-" + rootDevice.ToString();
outputRotation = RUISCalibrationResultsInQuaternion[devicePairString] * outputRotation;
}
// Apply floor pitch rotation (which is identity to anything else than Kinect 1/2)
if (applyToRootCoordinates || device == rootDevice)
{
outputRotation = RUISCalibrationResultsFloorPitchRotation[rootDevice] * outputRotation;
}
else
{
if (device == RUISDevice.Kinect_2)
{
outputRotation = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_2] * outputRotation;
}
else if (device == RUISDevice.Kinect_1)
{
outputRotation = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_1] * outputRotation;
}
}
// Apply yaw offset
if (applyToRootCoordinates || device == rootDevice)
{
outputRotation = Quaternion.Euler(0, yawOffset, 0) * outputRotation;
}
return(outputRotation);
}
public Vector3 ConvertMoveVelocity(Vector3 velocity)
{
//flip the z coordinate to get into unity's coordinate system
Vector3 newVelocity = new Vector3(velocity.x, velocity.y, -velocity.z);
string devicePairString = RUISDevice.PS_Move.ToString() + "-" + rootDevice.ToString();
if (applyToRootCoordinates && rootDevice != RUISDevice.PS_Move)
{
newVelocity = RUISCalibrationResultsIn4x4Matrix[devicePairString].MultiplyPoint3x4(newVelocity);
}
newVelocity *= moveToUnityScale;
newVelocity = Quaternion.Euler(0, yawOffset, 0) * newVelocity;
return(newVelocity);
}
/// <summary>
/// Convert location obtained with a certain device to master coordinate system, apply position offset, and set Kinect origin to floor if applicable
/// </summary>
public Vector3 ConvertLocation(Vector3 inputLocation, RUISDevice device)
{
Vector3 outputLocation = inputLocation;
// Transform location into master coordinate system
if (applyToRootCoordinates && rootDevice != device)
{
string devicePairString = device.ToString() + "-" + rootDevice.ToString();
outputLocation = RUISCalibrationResultsIn4x4Matrix[devicePairString].MultiplyPoint3x4(outputLocation);
// outputLocation = RUISCalibrationResultsInQuaternion[devicePairString] * outputLocation;
}
// Apply yaw offset and floor pitch rotation (which is identity to anything else than Kinect 1/2)
if (applyToRootCoordinates || device == rootDevice)
{
outputLocation = Quaternion.Euler(0, yawOffset, 0) * RUISCalibrationResultsFloorPitchRotation[rootDevice] * outputLocation;
}
else
{
if (device == RUISDevice.Kinect_2)
{
outputLocation = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_2] * outputLocation;
}
else if (device == RUISDevice.Kinect_1)
{
outputLocation = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_1] * outputLocation;
}
}
// Set Kinect 1/2 origin to floor
if (setKinectOriginToFloor)
{
if (applyToRootCoordinates || device == rootDevice)
{
outputLocation.y += RUISCalibrationResultsDistanceFromFloor[rootDevice];
}
else
{
if (device == RUISDevice.Kinect_2)
{
outputLocation.y += RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_2];
}
else if (device == RUISDevice.Kinect_1)
{
outputLocation.y += RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_1];
}
}
}
// Position offset
if (applyToRootCoordinates || device == rootDevice)
{
outputLocation += positionOffset;
}
return(outputLocation);
}
/// <summary>
/// Returns an approximation of Vector3 localScale (calculated from RUISCalibrationResultsIn4x4Matrix[devicePairString]),
/// which can be used to compensate the scale difference between the master coordinate system frame and the argument device frame.
/// </summary>
public Vector3 ExtractLocalScale(RUISDevice device)
{
if (applyToRootCoordinates && rootDevice != device)
{
string devicePairString = device.ToString() + "-" + rootDevice.ToString();
Matrix4x4 matrix = RUISCalibrationResultsIn4x4Matrix[devicePairString];
// Extract new local scale
return(new Vector3(matrix.GetColumn(0).magnitude, matrix.GetColumn(1).magnitude, matrix.GetColumn(2).magnitude));
}
else
{
return(Vector3.one);
}
}
public void SetDistanceFromFloor(float distance, RUISDevice floorDetectingDevice)
{
switch (floorDetectingDevice)
{
case RUISDevice.Kinect_1:
RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_1] = distance;
break;
case RUISDevice.Kinect_2:
RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_2] = distance;
break;
default:
Debug.LogWarning("Currently floor distance detection with " + floorDetectingDevice.ToString() + " is not supported!");
break;
}
}
public void updateDictionaries(Dictionary <string, Vector3> RUISCalibrationResultsInVector3,
Dictionary <string, Quaternion> RUISCalibrationResultsInQuaternion,
Dictionary <string, Matrix4x4> RUISCalibrationResultsIn4x4Matrix,
Vector3 translate, Quaternion rotation, Matrix4x4 pairwiseTransform,
RUISDevice device1, RUISDevice device2)
{
RUISCalibrationResultsInVector3[device1.ToString() + "-" + device2.ToString()] = translate;
RUISCalibrationResultsIn4x4Matrix[device1.ToString() + "-" + device2.ToString()] = pairwiseTransform;
RUISCalibrationResultsInQuaternion[device1.ToString() + "-" + device2.ToString()] = rotation;
// Inverses
RUISCalibrationResultsInVector3[device2.ToString() + "-" + device1.ToString()] = -translate;
RUISCalibrationResultsIn4x4Matrix[device2.ToString() + "-" + device1.ToString()] = pairwiseTransform.inverse;
RUISCalibrationResultsInQuaternion[device2.ToString() + "-" + device1.ToString()] = Quaternion.Inverse(rotation);
}
public void updateDictionaries(Dictionary<string, Vector3> RUISCalibrationResultsInVector3,
Dictionary<string, Quaternion> RUISCalibrationResultsInQuaternion,
Dictionary<string, Matrix4x4> RUISCalibrationResultsIn4x4Matrix,
Vector3 translate, Quaternion rotation, Matrix4x4 pairwiseTransform,
RUISDevice device1, RUISDevice device2)
{
RUISCalibrationResultsInVector3[device1.ToString() + "-" + device2.ToString()] = translate;
RUISCalibrationResultsIn4x4Matrix[device1.ToString() + "-" + device2.ToString()] = pairwiseTransform;
RUISCalibrationResultsInQuaternion[device1.ToString() + "-" + device2.ToString()] = rotation;
// Inverses
RUISCalibrationResultsInVector3[device2.ToString() + "-" + device1.ToString()] = -translate;
RUISCalibrationResultsIn4x4Matrix[device2.ToString() + "-" + device1.ToString()] = pairwiseTransform.inverse;
RUISCalibrationResultsInQuaternion[device2.ToString() + "-" + device1.ToString()] = Quaternion.Inverse(rotation);
}
public void SetFloorNormal(Vector3 newFloorNormal, RUISDevice floorDetectingDevice)
{
Quaternion kinectFloorRotator = Quaternion.identity;
kinectFloorRotator.SetFromToRotation(newFloorNormal, Vector3.up);
kinectFloorRotator = Quaternion.Inverse(kinectFloorRotator);
switch (floorDetectingDevice)
{
case RUISDevice.Kinect_1:
RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_1] = kinectFloorRotator;
break;
case RUISDevice.Kinect_2:
RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_2] = kinectFloorRotator;
break;
default:
Debug.LogWarning("Currently floor normal detection with " + floorDetectingDevice.ToString() + " is not supported!");
break;
}
}
public void SaveFloorData(string filename, RUISDevice device, Vector3 normal, float distance)
{
string wrapperElementName = device.ToString();
XmlNode calibrationMatrixElement;
XmlDocument xmlDoc = XMLUtil.LoadAndValidateXml(filename, coordinateSchema);
XmlNode groupElement;
if(xmlDoc != null) {
calibrationMatrixElement = xmlDoc.DocumentElement;
groupElement = calibrationMatrixElement.SelectSingleNode("FloorData");
if(groupElement == null) groupElement = xmlDoc.CreateElement("FloorData");
}
else {
xmlDoc = new XmlDocument();
xmlDoc.CreateXmlDeclaration("1.0", "UTF-8", "yes");
calibrationMatrixElement = xmlDoc.CreateElement("ns2", "RUISClibrationMatrix", "http://ruisystem.net/m2k");
xmlDoc.AppendChild(calibrationMatrixElement);
groupElement = xmlDoc.CreateElement("FloorData");
}
calibrationMatrixElement.AppendChild(groupElement);
XmlElement wrapperElement = xmlDoc.CreateElement(wrapperElementName);
groupElement.AppendChild(wrapperElement);
XmlElement kinectFloorNormalElement = xmlDoc.CreateElement("floorNormal");
kinectFloorNormalElement.SetAttribute("x", normal.x.ToString());
kinectFloorNormalElement.SetAttribute("y", normal.y.ToString());
kinectFloorNormalElement.SetAttribute("z", normal.z.ToString());
wrapperElement.AppendChild(kinectFloorNormalElement);
XmlElement kinectDistanceFromFloorElement = xmlDoc.CreateElement("distanceFromFloor");
kinectDistanceFromFloorElement.SetAttribute("value", distance.ToString());
wrapperElement.AppendChild(kinectDistanceFromFloorElement);
XmlNode groupNode = xmlDoc.DocumentElement.SelectSingleNode("FloorData");
XmlNode testNode = groupNode.SelectSingleNode(wrapperElementName);
// Element not found
if(testNode == null) {
groupNode.AppendChild(wrapperElement);
}
else {// Element already exists
var oldElem = testNode;
groupNode.ReplaceChild(wrapperElement, oldElem);
}
FileStream xmlFileStream = File.Open(filename, FileMode.Create);
StreamWriter streamWriter = new StreamWriter(xmlFileStream);
xmlDoc.Save(streamWriter);
streamWriter.Flush();
streamWriter.Close();
xmlFileStream.Close();
}
public void SaveTransformDataToXML(string filename, RUISDevice device1, RUISDevice device2)
{
string wrapperElementName = device1.ToString() + "-" + device2.ToString();
XmlNode calibrationMatrixElement;
XmlDocument xmlDoc = XMLUtil.LoadAndValidateXml(filename, coordinateSchema);
XmlNode groupElement;
if (xmlDoc != null)
{
calibrationMatrixElement = xmlDoc.DocumentElement;
groupElement = calibrationMatrixElement.SelectSingleNode("Transforms");
if (groupElement == null)
{
groupElement = xmlDoc.CreateElement("Transforms");
}
}
else
{
xmlDoc = new XmlDocument();
xmlDoc.CreateXmlDeclaration("1.0", "UTF-8", "yes");
calibrationMatrixElement = xmlDoc.CreateElement("ns2", "RUISClibrationMatrix", "http://ruisystem.net/m2k");
xmlDoc.AppendChild(calibrationMatrixElement);
groupElement = xmlDoc.CreateElement("Transforms");
}
calibrationMatrixElement.AppendChild(groupElement);
XmlElement wrapperElement = xmlDoc.CreateElement(wrapperElementName);
groupElement.AppendChild(wrapperElement);
XmlElement translateElement = xmlDoc.CreateElement("translate");
translateElement.SetAttribute("x", deviceToRootTransform[0, 3].ToString());
translateElement.SetAttribute("y", deviceToRootTransform[1, 3].ToString());
translateElement.SetAttribute("z", deviceToRootTransform[2, 3].ToString());
wrapperElement.AppendChild(translateElement);
XmlElement rotateElement = xmlDoc.CreateElement("rotate");
rotateElement.SetAttribute("r00", deviceToRootTransform[0, 0].ToString());
rotateElement.SetAttribute("r01", deviceToRootTransform[0, 1].ToString());
rotateElement.SetAttribute("r02", deviceToRootTransform[0, 2].ToString());
rotateElement.SetAttribute("r10", deviceToRootTransform[1, 0].ToString());
rotateElement.SetAttribute("r11", deviceToRootTransform[1, 1].ToString());
rotateElement.SetAttribute("r12", deviceToRootTransform[1, 2].ToString());
rotateElement.SetAttribute("r20", deviceToRootTransform[2, 0].ToString());
rotateElement.SetAttribute("r21", deviceToRootTransform[2, 1].ToString());
rotateElement.SetAttribute("r22", deviceToRootTransform[2, 2].ToString());
wrapperElement.AppendChild(rotateElement);
XmlNode groupNode = xmlDoc.DocumentElement.SelectSingleNode("Transforms");
XmlNode testNode = groupNode.SelectSingleNode(wrapperElementName);
// Element not found
if (testNode == null)
{
groupNode.AppendChild(wrapperElement);
}
else
{
// Element already exists
var oldElem = testNode;
groupNode.ReplaceChild(wrapperElement, oldElem);
}
FileStream xmlFileStream = File.Open(filename, FileMode.Create);
StreamWriter streamWriter = new StreamWriter(xmlFileStream);
xmlDoc.Save(streamWriter);
streamWriter.Flush();
streamWriter.Close();
xmlFileStream.Close();
}
/*
* Convert rotations obtained with a certain device to master coordinate system, apply yaw offset, and apply Kinect pitch correction
*/
public Quaternion ConvertRotation(Quaternion inputRotation, RUISDevice device)
{
Quaternion outputRotation = inputRotation;
if (applyToRootCoordinates && rootDevice != device)
{
string devicePairString = device.ToString() + "-" + rootDevice.ToString();
outputRotation = RUISCalibrationResultsInQuaternion[devicePairString] * outputRotation;
}
// Apply floor pitch rotation (which is identity to anything else than Kinect 1/2)
if (applyToRootCoordinates || device == rootDevice)
outputRotation = RUISCalibrationResultsFloorPitchRotation[rootDevice] * outputRotation;
else
{
if(device == RUISDevice.Kinect_2)
outputRotation = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_2] * outputRotation;
else if(device == RUISDevice.Kinect_1)
outputRotation = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_1] * outputRotation;
}
// Apply yaw offset
if (applyToRootCoordinates || device == rootDevice)
outputRotation = Quaternion.Euler(0, yawOffset, 0) * outputRotation;
return outputRotation;
}
/*
* Convert locations obtained with a certain device to master coordinate system, apply position offset, and set Kinect origin to floor if applicable
*/
public Vector3 ConvertLocation(Vector3 inputLocation, RUISDevice device)
{
Vector3 outputLocation = inputLocation;
// Transform location into master coordinate system
if (applyToRootCoordinates && rootDevice != device)
{
string devicePairString = device.ToString() + "-" + rootDevice.ToString();
outputLocation = RUISCalibrationResultsIn4x4Matrix[devicePairString].MultiplyPoint3x4(outputLocation);
}
// Apply yaw offset and floor pitch rotation (which is identity to anything else than Kinect 1/2)
if (applyToRootCoordinates || device == rootDevice)
outputLocation = Quaternion.Euler(0, yawOffset, 0) * RUISCalibrationResultsFloorPitchRotation[rootDevice] * outputLocation;
else
{
if(device == RUISDevice.Kinect_2)
outputLocation = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_2] * outputLocation;
else if(device == RUISDevice.Kinect_1)
outputLocation = RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_1] * outputLocation;
}
// Set Kinect 1/2 origin to floor
if (setKinectOriginToFloor)
{
if (applyToRootCoordinates || device == rootDevice)
outputLocation.y += RUISCalibrationResultsDistanceFromFloor[rootDevice];
else
{
if(device == RUISDevice.Kinect_2)
outputLocation.y += RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_2];
else if(device == RUISDevice.Kinect_1)
outputLocation.y += RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_1];
}
}
// Position offset
if (applyToRootCoordinates || device == rootDevice)
outputLocation += positionOffset;
return outputLocation;
}
public void SetDistanceFromFloor(float distance, RUISDevice floorDetectingDevice)
{
switch(floorDetectingDevice)
{
case RUISDevice.Kinect_1:
RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_1] = distance;
break;
case RUISDevice.Kinect_2:
RUISCalibrationResultsDistanceFromFloor[RUISDevice.Kinect_2] = distance;
break;
default:
Debug.LogWarning("Currently floor distance detection with " + floorDetectingDevice.ToString() + " is not supported!");
break;
}
}
public void SetFloorNormal(Vector3 newFloorNormal, RUISDevice floorDetectingDevice)
{
Quaternion kinectFloorRotator = Quaternion.identity;
kinectFloorRotator.SetFromToRotation(newFloorNormal, Vector3.up);
kinectFloorRotator = Quaternion.Inverse(kinectFloorRotator);
switch(floorDetectingDevice)
{
case RUISDevice.Kinect_1:
RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_1] = kinectFloorRotator;
break;
case RUISDevice.Kinect_2:
RUISCalibrationResultsFloorPitchRotation[RUISDevice.Kinect_2] = kinectFloorRotator;
break;
default:
Debug.LogWarning("Currently floor normal detection with " + floorDetectingDevice.ToString() + " is not supported!");
break;
}
}
void Start()
{
if ((firstDevice == RUISDevice.Kinect_2 && secondDevice == RUISDevice.Oculus_DK2) ||
(secondDevice == RUISDevice.Kinect_2 && firstDevice == RUISDevice.Oculus_DK2))
{
skeletonController.bodyTrackingDeviceID = RUISSkeletonManager.kinect2SensorID;
coordinateSystem.rootDevice = RUISDevice.Kinect_2;
calibrationProcess = new RUISKinect2ToOculusDK2CalibrationProcess(calibrationProcessSettings);
}
else if ((firstDevice == RUISDevice.Kinect_1 && secondDevice == RUISDevice.Kinect_2) ||
(secondDevice == RUISDevice.Kinect_1 && firstDevice == RUISDevice.Kinect_2))
{
skeletonController.bodyTrackingDeviceID = RUISSkeletonManager.kinect1SensorID;
coordinateSystem.rootDevice = RUISDevice.Kinect_1;
calibrationProcess = new RUISKinect2ToKinectCalibrationProcess(calibrationProcessSettings);
}
else if ((firstDevice == RUISDevice.Kinect_1 && secondDevice == RUISDevice.PS_Move) ||
(secondDevice == RUISDevice.Kinect_1 && firstDevice == RUISDevice.PS_Move))
{
skeletonController.bodyTrackingDeviceID = RUISSkeletonManager.kinect1SensorID;
coordinateSystem.rootDevice = RUISDevice.Kinect_1;
calibrationProcess = new RUISKinectToPSMoveCalibrationProcess(calibrationProcessSettings);
}
else if ((firstDevice == RUISDevice.Kinect_2 && secondDevice == RUISDevice.PS_Move) ||
(secondDevice == RUISDevice.Kinect_2 && firstDevice == RUISDevice.PS_Move))
{
skeletonController.bodyTrackingDeviceID = RUISSkeletonManager.kinect2SensorID;
coordinateSystem.rootDevice = RUISDevice.Kinect_2;
calibrationProcess = new RUISKinect2ToPSMoveCalibrationProcess(calibrationProcessSettings);
}
else if ((firstDevice == RUISDevice.PS_Move && secondDevice == RUISDevice.Oculus_DK2) ||
(secondDevice == RUISDevice.PS_Move && firstDevice == RUISDevice.Oculus_DK2))
{
skeletonController.bodyTrackingDeviceID = RUISSkeletonManager.customSensorID;
coordinateSystem.rootDevice = RUISDevice.Oculus_DK2;
calibrationProcess = new RUISPSMoveToOculusDK2CalibrationProcess(calibrationProcessSettings);
}
else if ((firstDevice == RUISDevice.Kinect_1 && secondDevice == RUISDevice.Oculus_DK2) ||
(secondDevice == RUISDevice.Kinect_1 && firstDevice == RUISDevice.Oculus_DK2))
{
skeletonController.bodyTrackingDeviceID = RUISSkeletonManager.kinect1SensorID;
coordinateSystem.rootDevice = RUISDevice.Kinect_1;
calibrationProcess = new RUISKinectToOculusDK2CalibrationProcess(calibrationProcessSettings);
}
else if (firstDevice == RUISDevice.Kinect_1 && secondDevice == RUISDevice.Kinect_1)
{
skeletonController.bodyTrackingDeviceID = RUISSkeletonManager.kinect1SensorID;
coordinateSystem.rootDevice = RUISDevice.Kinect_1;
calibrationProcess = new RUISKinectFloorDataCalibrationProcess(calibrationProcessSettings);
}
else if (firstDevice == RUISDevice.Kinect_2 && secondDevice == RUISDevice.Kinect_2)
{
skeletonController.bodyTrackingDeviceID = RUISSkeletonManager.kinect2SensorID;
coordinateSystem.rootDevice = RUISDevice.Kinect_2;
calibrationProcess = new RUISKinect2FloorDataCalibrationProcess(calibrationProcessSettings);
}
else
{
calibrationProcess = null;
}
if (calibrationProcess == null)
{
upperText.text = "";
lowerText.text = "Selected calibration device combination\n not yet supported.";
foreach (Transform child in this.deviceModels.transform)
{
child.gameObject.SetActive(false);
}
foreach (Transform child in this.depthViews.transform)
{
child.gameObject.SetActive(false);
}
foreach (Transform child in this.icons.transform)
{
child.gameObject.SetActive(false);
}
this.calibrationResultPhaseObjects.SetActive(false);
currentPhase = RUISCalibrationPhase.Invalid;
}
else
{
currentPhase = RUISCalibrationPhase.Initial;
}
string devicePairName = firstDevice.ToString() + "-" + secondDevice.ToString();
string devicePairName2 = secondDevice.ToString() + "-" + firstDevice.ToString();
coordinateSystem.RUISCalibrationResultsIn4x4Matrix[devicePairName] = Matrix4x4.identity;
coordinateSystem.RUISCalibrationResultsDistanceFromFloor[firstDevice] = 0.0f;
coordinateSystem.RUISCalibrationResultsFloorPitchRotation[firstDevice] = Quaternion.identity;
coordinateSystem.RUISCalibrationResultsIn4x4Matrix[devicePairName2] = Matrix4x4.identity;
coordinateSystem.RUISCalibrationResultsDistanceFromFloor[secondDevice] = 0.0f;
coordinateSystem.RUISCalibrationResultsFloorPitchRotation[secondDevice] = Quaternion.identity;
}
public void SaveFloorData(string filename, RUISDevice device, Vector3 normal, float distance)
{
string wrapperElementName = device.ToString();
XmlNode calibrationMatrixElement;
XmlDocument xmlDoc = XMLUtil.LoadAndValidateXml(filename, coordinateSchema);
XmlNode groupElement;
if (xmlDoc != null)
{
calibrationMatrixElement = xmlDoc.DocumentElement;
groupElement = calibrationMatrixElement.SelectSingleNode("FloorData");
if (groupElement == null)
{
groupElement = xmlDoc.CreateElement("FloorData");
}
}
else
{
xmlDoc = new XmlDocument();
xmlDoc.CreateXmlDeclaration("1.0", "UTF-8", "yes");
calibrationMatrixElement = xmlDoc.CreateElement("ns2", "RUISClibrationMatrix", "http://ruisystem.net/m2k");
xmlDoc.AppendChild(calibrationMatrixElement);
groupElement = xmlDoc.CreateElement("FloorData");
}
calibrationMatrixElement.AppendChild(groupElement);
XmlElement wrapperElement = xmlDoc.CreateElement(wrapperElementName);
groupElement.AppendChild(wrapperElement);
XmlElement kinectFloorNormalElement = xmlDoc.CreateElement("floorNormal");
kinectFloorNormalElement.SetAttribute("x", normal.x.ToString());
kinectFloorNormalElement.SetAttribute("y", normal.y.ToString());
kinectFloorNormalElement.SetAttribute("z", normal.z.ToString());
wrapperElement.AppendChild(kinectFloorNormalElement);
XmlElement kinectDistanceFromFloorElement = xmlDoc.CreateElement("distanceFromFloor");
kinectDistanceFromFloorElement.SetAttribute("value", distance.ToString());
wrapperElement.AppendChild(kinectDistanceFromFloorElement);
XmlNode groupNode = xmlDoc.DocumentElement.SelectSingleNode("FloorData");
XmlNode testNode = groupNode.SelectSingleNode(wrapperElementName);
// Element not found
if (testNode == null)
{
groupNode.AppendChild(wrapperElement);
}
else
{
// Element already exists
var oldElem = testNode;
groupNode.ReplaceChild(wrapperElement, oldElem);
}
FileStream xmlFileStream = File.Open(filename, FileMode.Create);
StreamWriter streamWriter = new StreamWriter(xmlFileStream);
xmlDoc.Save(streamWriter);
streamWriter.Flush();
streamWriter.Close();
xmlFileStream.Close();
}
public void SaveTransformDataToXML(string filename, RUISDevice device1, RUISDevice device2)
{
string wrapperElementName = device1.ToString() + "-" + device2.ToString();
XmlNode calibrationMatrixElement;
XmlDocument xmlDoc = XMLUtil.LoadAndValidateXml(filename, coordinateSchema);
XmlNode groupElement;
if(xmlDoc != null) {
calibrationMatrixElement = xmlDoc.DocumentElement;
groupElement = calibrationMatrixElement.SelectSingleNode("Transforms");
if(groupElement == null) groupElement = xmlDoc.CreateElement("Transforms");
}
else {
xmlDoc = new XmlDocument();
xmlDoc.CreateXmlDeclaration("1.0", "UTF-8", "yes");
calibrationMatrixElement = xmlDoc.CreateElement("ns2", "RUISClibrationMatrix", "http://ruisystem.net/m2k");
xmlDoc.AppendChild(calibrationMatrixElement);
groupElement = xmlDoc.CreateElement("Transforms");
}
calibrationMatrixElement.AppendChild(groupElement);
XmlElement wrapperElement = xmlDoc.CreateElement(wrapperElementName);
groupElement.AppendChild(wrapperElement);
XmlElement translateElement = xmlDoc.CreateElement("translate");
translateElement.SetAttribute("x", deviceToRootTransform[0, 3].ToString());
translateElement.SetAttribute("y", deviceToRootTransform[1, 3].ToString());
translateElement.SetAttribute("z", deviceToRootTransform[2, 3].ToString());
wrapperElement.AppendChild(translateElement);
XmlElement rotateElement = xmlDoc.CreateElement("rotate");
rotateElement.SetAttribute("r00", deviceToRootTransform[0, 0].ToString());
rotateElement.SetAttribute("r01", deviceToRootTransform[0, 1].ToString());
rotateElement.SetAttribute("r02", deviceToRootTransform[0, 2].ToString());
rotateElement.SetAttribute("r10", deviceToRootTransform[1, 0].ToString());
rotateElement.SetAttribute("r11", deviceToRootTransform[1, 1].ToString());
rotateElement.SetAttribute("r12", deviceToRootTransform[1, 2].ToString());
rotateElement.SetAttribute("r20", deviceToRootTransform[2, 0].ToString());
rotateElement.SetAttribute("r21", deviceToRootTransform[2, 1].ToString());
rotateElement.SetAttribute("r22", deviceToRootTransform[2, 2].ToString());
wrapperElement.AppendChild(rotateElement);
XmlNode groupNode = xmlDoc.DocumentElement.SelectSingleNode("Transforms");
XmlNode testNode = groupNode.SelectSingleNode(wrapperElementName);
// Element not found
if(testNode == null) {
groupNode.AppendChild(wrapperElement);
}
else {// Element already exists
var oldElem = testNode;
groupNode.ReplaceChild(wrapperElement, oldElem);
}
FileStream xmlFileStream = File.Open(filename, FileMode.Create);
StreamWriter streamWriter = new StreamWriter(xmlFileStream);
xmlDoc.Save(streamWriter);
streamWriter.Flush();
streamWriter.Close();
xmlFileStream.Close();
}
