private void updateKinectFloorData()
{
if (coordinateSystem)
{
if (coordinateSystem.rootDevice == RUISDevice.Kinect_1)
{
coordinateSystem.ResetFloorNormal(RUISDevice.Kinect_1);
coordinateSystem.ResetDistanceFromFloor(RUISDevice.Kinect_1);
}
OpenNI.Plane3D floor;
try
{
floor = sceneAnalyzer.Floor;
}
catch (System.Exception e)
{
Debug.LogError(e.TargetSite + ": Failed to get OpenNI.SceneAnalyzer.Floor.");
return;
}
Vector3 newFloorNormal = new Vector3(floor.Normal.X, floor.Normal.Y, floor.Normal.Z).normalized;
Vector3 newFloorPosition = (new Vector3(floor.Point.X, floor.Point.Y, floor.Point.Z)) * RUISCoordinateSystem.kinectToUnityScale;
//Vector3 newFloorPosition = coordinateSystem.ConvertKinectPosition(floor.Point);
//Project the position of the kinect camera onto the floor
//http://en.wikipedia.org/wiki/Point_on_plane_closest_to_origin
//http://en.wikipedia.org/wiki/Plane_(geometry)
float d = newFloorNormal.x * newFloorPosition.x + newFloorNormal.y * newFloorPosition.y + newFloorNormal.z * newFloorPosition.z;
Vector3 closestFloorPointToKinect = new Vector3(newFloorNormal.x, newFloorNormal.y, newFloorNormal.z);
closestFloorPointToKinect = (closestFloorPointToKinect * d) / closestFloorPointToKinect.sqrMagnitude;
//floorPlane.transform.position = closestFloorPointToKinect;
Quaternion kinectFloorRotator = Quaternion.FromToRotation(newFloorNormal, Vector3.up);
//transform the point from Kinect's coordinate system rotation to Unity's rotation
closestFloorPointToKinect = kinectFloorRotator * closestFloorPointToKinect;
if (float.IsNaN(closestFloorPointToKinect.magnitude))
{
closestFloorPointToKinect = Vector3.zero;
}
if (newFloorNormal.sqrMagnitude < 0.1f)
{
newFloorNormal = Vector3.up;
}
if (coordinateSystem.rootDevice == RUISDevice.Kinect_1)
{
coordinateSystem.SetFloorNormal(newFloorNormal, RUISDevice.Kinect_1);
coordinateSystem.SetDistanceFromFloor(closestFloorPointToKinect.magnitude, RUISDevice.Kinect_1);
}
Debug.Log("Updated Kinect floor normal " + newFloorNormal + " and floor distance (" + closestFloorPointToKinect.magnitude + ")");
}
}