private static double getCameraAngle(Windows.Kinect.Vector4 _floor)
{
double cameraAngleRadians = System.Math.Atan(_floor.Z / _floor.Y);
// return System.Math.Cos(cameraAngleRadians);
return(cameraAngleRadians * 180 / System.Math.PI);
}
private void UpdateFloorNormalAndDistance()
{
coordinateSystem.ResetFloorNormal(RUISDevice.Kinect_2);
Windows.Kinect.Vector4 kinect2FloorPlane = kinect2SourceManager.GetFlootClipPlane();
kinect2FloorNormal = new Vector3(kinect2FloorPlane.X, kinect2FloorPlane.Y, kinect2FloorPlane.Z);
kinect2FloorNormal.Normalize();
if (kinect2FloorNormal.sqrMagnitude < 0.1f)
{
kinect2FloorNormal = Vector3.up;
}
kinect2DistanceFromFloor = kinect2FloorPlane.W / Mathf.Sqrt(kinect2FloorNormal.sqrMagnitude);
if (float.IsNaN(kinect2DistanceFromFloor))
{
kinect2DistanceFromFloor = 0;
}
Quaternion kinect2FloorRotator = Quaternion.FromToRotation(kinect2FloorNormal, Vector3.up);
kinect2PitchRotation = Quaternion.Inverse(kinect2FloorRotator);
coordinateSystem.SetDistanceFromFloor(kinect2DistanceFromFloor, RUISDevice.Kinect_2);
coordinateSystem.SetFloorNormal(kinect2FloorNormal, RUISDevice.Kinect_2);
}
void Start()
{
_Sensor = Windows.Kinect.KinectSensor.GetDefault();
if (_Sensor != null)
{
_Reader = _Sensor.BodyFrameSource.OpenReader();
if (!_Sensor.IsOpen)
{
_Sensor.Open();
}
isActive = true;
// Floor
_Reader.FrameArrived += (object sender, BodyFrameArrivedEventArgs e) =>
{
using(var frame = e.FrameReference.AcquireFrame())
{
this.currentTime = frame.RelativeTime;
this.currentFloor = frame.FloorClipPlane;
}
};
}
}
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
{
Debug.Log(body.Joints [Kinect.JointType.Head].Position.Y - body.Joints [Kinect.JointType.FootLeft].Position.Y);
for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.ThumbRight; jt++)
{
Kinect.Joint sourceJoint = body.Joints[jt];
Kinect.Joint?targetJoint = null;
if (_BoneMap.ContainsKey(jt))
{
targetJoint = body.Joints[_BoneMap[jt]];
}
Transform jointObj = bodyObject.transform.Find(jt.ToString());
jointObj.localPosition = Vector3.MoveTowards(jointObj.localPosition, GetVector3FromJoint(sourceJoint), 100 * Time.deltaTime);
if (jt.Equals(Kinect.JointType.HandRight))
{
Windows.Kinect.Vector4 orientation = body.JointOrientations [jt].Orientation;
jointObj.localRotation = Quaternion.RotateTowards(jointObj.localRotation, new Quaternion(orientation.X, orientation.Y, orientation.Z, orientation.W), 800 * Time.deltaTime);
//jointObj.Rotate(0, 90, 0);
}
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
if (targetJoint.HasValue)
{
lr.SetPosition(0, jointObj.localPosition);
lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value));
lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState));
}
else
{
lr.enabled = false;
}
}
}
private static Vector3 GetFloorClipPlane(EJoint jointhead, Windows.Kinect.Vector4 _floor)
{
return(new Vector3(
(float)0,
(float)-(_floor.X * jointhead.Position.X + _floor.Z * jointhead.Position.Z + _floor.W) / _floor.Y * 10,
(float)0
));
}
private void UpdateFloorNormalAndDistance()
{
coordinateSystem.ResetFloorNormal(RUISDevice.Kinect_2);
Windows.Kinect.Vector4 kinect2FloorPlane = kinect2SourceManager.GetFlootClipPlane();
kinect2FloorNormal = new Vector3(kinect2FloorPlane.X, kinect2FloorPlane.Y, kinect2FloorPlane.Z);
kinect2FloorNormal.Normalize();
if (kinect2FloorNormal.sqrMagnitude < 0.1f)
{
kinect2FloorNormal = Vector3.up;
}
kinect2DistanceFromFloor = kinect2FloorPlane.W / Mathf.Sqrt(kinect2FloorNormal.sqrMagnitude);
if (float.IsNaN(kinect2DistanceFromFloor))
{
kinect2DistanceFromFloor = 0;
}
// Quaternion kinect2FloorRotator = Quaternion.FromToRotation(kinect2FloorNormal, Vector3.up);
// kinect2PitchRotation = Quaternion.Inverse (kinect2FloorRotator);
coordinateSystem.SetDistanceFromFloor(kinect2DistanceFromFloor, RUISDevice.Kinect_2);
coordinateSystem.SetFloorNormal(kinect2FloorNormal, RUISDevice.Kinect_2);
OpenNI.Plane3D floor;
try{
floor = sceneAnalyzer.Floor;
}
catch (System.Exception e)
{
Debug.LogError(e.TargetSite + ": Failed to get OpenNI.SceneAnalyzer.Floor.");
return;
//throw e;
}
Quaternion kinectFloorRotator = Quaternion.identity;
kinect1FloorNormal = new Vector3(floor.Normal.X, floor.Normal.Y, floor.Normal.Z);
if (kinect1FloorNormal.sqrMagnitude < 0.1f)
{
kinect1FloorNormal = Vector3.up;
}
Vector3 floorPoint = new Vector3(floor.Point.X, floor.Point.Y, floor.Point.Z);
kinectFloorRotator = Quaternion.FromToRotation(kinect1FloorNormal, Vector3.up);
kinect1DistanceFromFloor = closestDistanceFromFloor(kinect1FloorNormal, floorPoint, RUISCoordinateSystem.kinectToUnityScale);
kinect1PitchRotation = Quaternion.Inverse(kinectFloorRotator);
}
void _floorPlaneNewFrame(object sender, BodyFrame frame)
{
if (_bodySourceManager.getReader () != null) {
if (frame != null) {
_vec4 = frame.FloorClipPlane;
//Debug.Log(_vec4.W);
_plane.transform.position = new Vector3 (-4, _vec4.W * (-1), 1);
//_plane.transform.Translate(new Vector3(0.1f,0.1f,0.1f));
}
}
}
void _floorPlaneNewFrame(object sender, BodyFrame frame)
{
if (_bodySourceManager.getReader() != null)
{
if (frame != null)
{
_vec4 = frame.FloorClipPlane;
//Debug.Log(_vec4.W);
_plane.transform.position = new Vector3(-4, _vec4.W * (-1), 1);
//_plane.transform.Translate(new Vector3(0.1f,0.1f,0.1f));
}
}
}
void _floorPlaneNewFrame(object sender, BodyFrame frame)
{
if (_bodySourceManager.getReader () != null) {
if (frame != null) {
_vec4 = frame.FloorClipPlane;
//Debug.Log(_vec4.W);
_plane.transform.position = new Vector3 (-4, _vec4.W * (-1), 5);
Probe.transform.position =_plane.transform.position +adjustment;
Room.transform.position = _plane.transform.position + adjustment;
}
}
}
public Joint(Kinect.CameraSpacePoint position, Windows.Kinect.Vector4 rotation)
{
this.Position = new Vector3(position.X, position.Y, position.Z);
this.Rotation = new Quaternion(rotation.X, rotation.Y, rotation.Z, rotation.W);
}
private Quaternion VToQ(Windows.Kinect.Vector4 kinectQ, Quaternion comp)
{
return(Quaternion.Inverse(comp) * (new Quaternion(-kinectQ.X, -kinectQ.Y, kinectQ.Z, kinectQ.W)));
}
void Update()
{
if (BodySourcePlayer == null)
{
return;
}
_BodyManager = BodySourcePlayer.GetComponent <BodySourcePlayer>();
if (_BodyManager == null)
{
return;
}
EmitBody[] data = _BodyManager.EGetData();
_floorData = _BodyManager.FloorPlane;
if (data == null)
{
return;
}
List <ulong> trackedIds = new List <ulong>();
groundPosition = transform.position;
foreach (var body in data)
{
if (body == null)
{
continue;
}
if (body.IsTracked)
{
trackedIds.Add(body.TrackingId);
}
}
List <ulong> knownIds = new List <ulong>(_Bodies.Keys);
// First delete untracked bodies
foreach (ulong trackingId in knownIds)
{
if (!trackedIds.Contains(trackingId))
{
Destroy(_Bodies[trackingId]);
_Bodies.Remove(trackingId);
}
}
foreach (var body in data)
{
if (body == null)
{
continue;
}
if (body.IsTracked)
{
if (!_Bodies.ContainsKey(body.TrackingId))
{
_Bodies[body.TrackingId] = CreateBodyObject(body.TrackingId);
}
RefreshBodyObject(body, _Bodies[body.TrackingId]);
}
}
}
