public double CalculateDistanceByFormula(Windows.Kinect.Joint g1, Windows.Kinect.Joint g2) { Vector3 s = GetVector3FromJoint(g1); Vector3 t = GetVector3FromJoint(g2); return(Math.Sqrt(Math.Pow(s.x - t.x, 2) + Math.Pow(s.y - t.y, 2) + Math.Pow(s.z - t.z, 2))); }
void RefreshBodyObject(Windows.Kinect.Body body) { bool isGen = false; if (tt.Count - 1 != (int)Windows.Kinect.JointType.ThumbRight) { tt.Clear(); isGen = true; print("ボーンを生成します."); } for (Windows.Kinect.JointType jt = Windows.Kinect.JointType.SpineBase; jt <= Windows.Kinect.JointType.ThumbRight; jt++) { Windows.Kinect.Joint sourceJoint = body.Joints[jt]; Vector3 tmp = GetVector3FromJoint(sourceJoint) * 20; if (isGen) { var t = GameObject.CreatePrimitive(PrimitiveType.Cube).transform; t.name = jt.ToString(); t.position = tmp; t.parent = transform; Destroy(t.GetComponent <BoxCollider>()); tt.Add(t); } else { Transform t = tt[(int)jt]; if (sourceJoint.TrackingState == Windows.Kinect.TrackingState.Tracked) { t.position = tmp; // t.gameObject.SetActive(true); } else { // t.gameObject.SetActive(false); } } } }
private GameObject ConstructBone(Kinect.Joint source, Kinect.Joint dest) { // special cylinder instantiated between two points GameObject ob = GameObject.CreatePrimitive(PrimitiveType.Cylinder); if (ob.GetComponent <Collider>() != null) { Destroy(ob.GetComponent <Collider>()); } // fake (but clever) 2d physics GameObject child = new GameObject("collider", typeof(CapsuleCollider)); child.transform.SetParent(ob.transform); ob = UpdateBone(ob, source, dest); // always render on top //ob.layer = LayerMask.NameToLayer("KinectBody"); ob.GetComponent <Renderer>().material = boneMaterial; return(ob); }
private Vector3 GetVector3FromJoint(Kinect.Joint joint) { bool valid = joint.TrackingState != Kinect.TrackingState.NotTracked; if (convert_camera_ != null || valid) { Kinect.ColorSpacePoint point = coordinate_mapper_.MapCameraPointToColorSpace(joint.Position); Vector3 point2 = new Vector3(point.X, point.Y, 0); if ((point2.x >= 0) && (point2.x < kKinectWidth) && (point2.y >= 0) && (point2.y < kKinectHeight)) { point2.x = point2.x * Screen.width / kKinectWidth; point2.y = point2.y * Screen.height / kKinectHeight; Vector3 color_point3 = convert_camera_.ScreenToWorldPoint(point2); color_point3.y *= -1; color_point3.z = -1; return(color_point3); } } return(new Vector3(joint.Position.X * 10, joint.Position.Y * 10, joint.Position.Z * 10)); }
private Ranges inBlurArea(Kinect.Joint joint) { float px = joint.Position.X; float pz = joint.Position.Z; if (px < BLUR_START_MIN_X) { return(Ranges.LEFT); } else if (px > BLUR_START_MAX_X) { return(Ranges.RIGHT); } else if (pz < BLUR_START_MIN_Z) { return(Ranges.FRONT); } else if (pz > BLUR_START_MAX_Z) { return(Ranges.BACK); } return(Ranges.NONE); }
/// <summary> /// Refreshes the current body updating /// the position of each of the limbs /// </summary> /// <param name="body">The Body to Update</param> /// <param name="bodyObject">The Body object holding all the pieces</param> private void RefreshHandPosition(Kinect.Body body, GameObject bodyObject) { if (!initialized) { handCursor = Instantiate(handCursorObj, bodyObject.transform.position, Quaternion.identity); handCursorScript = handCursor.GetComponent <EnterNameHandCursor>(); playerSpawnPosition.localEulerAngles = new Vector3(0, playerSpawnPosition.localEulerAngles.y - 90, playerSpawnPosition.localEulerAngles.z + 90); initialized = true; } Kinect.Joint rh = body.Joints[Kinect.JointType.HandRight]; Transform currentJointObj = bodyObject.transform.Find(Kinect.JointType.HandRight.ToString()); /// Find the Vector3 Position of this joint Vector3 currentJointPosition = BodySourceManager.GetVector3FromJoint(rh); currentJointObj.localPosition = new Vector3(currentJointPosition.x * -1, currentJointPosition.y, currentJointPosition.z); Vector3 handPos = (currentJointObj.position - perspectiveObj.position) * 0.5f + perspectiveObj.position; des = new Vector3(handPos.x, 2, handPos.z); }
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject) { 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.FindChild(jt.ToString()); jointObj.localPosition = GetVector3FromJoint(sourceJoint); 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; } if (jt == Kinect.JointType.HandLeft) { LHand.transform.position = jointObj.transform.position; } if (jt == Kinect.JointType.HandRight) { RHand.transform.position = jointObj.transform.position; } } }
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject) { 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 = GetVector3FromJoint(sourceJoint); if (jt.ToString() == "SpineBase") { _boomStronk0.localPosition = GetVector3FromJoint(sourceJoint); _boomStronk0.localPosition = new Vector3(_boomStronk0.localPosition.x, _boomStronk0.localPosition.y, 0); // jointObj.transform.localScale = new Vector3(5, 5, 5); } if (jt.ToString() == "SpineMid") { _boomStronk1.localPosition = GetVector3FromJoint(sourceJoint); _boomStronk1.localPosition = new Vector3(_boomStronk1.localPosition.x, _boomStronk1.localPosition.y, 0); // jointObj.transform.localScale = new Vector3(5, 5, 5); } if (jt.ToString() == "Head") { _boomHoofd.localPosition = GetVector3FromJoint(sourceJoint); _boomHoofd.localPosition = new Vector3(_boomHoofd.localPosition.x, _boomHoofd.localPosition.y, 0); // jointObj.transform.localScale = new Vector3(5, 5, 5); } } }
/* private void Align_Body() * { * * }*/ private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject) { 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 = GetVector3FromJoint(sourceJoint); LineRenderer lr = jointObj.GetComponent <LineRenderer>(); if (targetJoint.HasValue) { lr.SetPosition(0, jointObj.position); lr.SetPosition(1, GetVector3FromScene(targetJoint.Value, bodyObject)); lr.SetColors(GetColorForState(sourceJoint.TrackingState), GetColorForState(targetJoint.Value.TrackingState)); } else { lr.enabled = false; } /* VIVE-KINECT CALIBRATION * if (jt == Kinect.JointType.Head) * { * jointObj.tag = "Head"; * Vector3 headOffset = GetVector3FromJoint(sourceJoint); //Offset of head node from body * bodyObject.transform.position = new Vector3(mainCamera.transform.position.x - headOffset.x, mainCamera.transform.position.y - headOffset.y, mainCamera.transform.position.z - headOffset.z); //Sets body pos * } * END */ } }
private static Vector3 ScaleXY(Vector3 orig, Windows.Kinect.Joint joint, Vector3 jointSpinePos) { float opp1X = joint.Position.X - jointSpinePos.x; float adj2 = orig.z; float adj1 = joint.Position.Z; float opp1Y = joint.Position.Y - jointSpinePos.y; float scaleX = opp1X * adj2 / adj1; float scaleY = opp1Y * adj2 / adj1; //if (joint.JointType.Equals(JointType.WristRight)) //{ // Debug.Log("Scaling for joint " + joint.JointType.ToString() + "..."); // Debug.Log("\tXScale = " + scaleX); // Debug.Log("\tYScale = " + scaleY); // Debug.Log("\tOrig X = " + orig.x); // Debug.Log("\tOrig Y = " + orig.y); // Debug.Log("\tjoint Pos.z = " + adj1); // Debug.Log("\tadj2 = " + adj2); //} return(orig + new Vector3(Mathf.Abs(orig.x * scaleX), Mathf.Abs(orig.y * scaleY), 0)); }
public void RefreshBodyObject() { foreach (JointType jt in Kinect.EssentialJoints) { Windows.Kinect.Joint sourceJoint = _trackedBody.Joints[jt]; if (Kinect.BoneMap.ContainsKey(jt)) { } Transform jointObj = BodyTransforms[jt]; Vector3 pos = GetVector3FromJoint(sourceJoint); BodyFilters[jt].record(pos); } foreach (JointType jt in Kinect.UnessentialJoints) { Windows.Kinect.Joint sourceJoint = _trackedBody.Joints[jt]; Transform jointObj = BodyTransforms[jt]; Vector3 pos = GetVector3FromJoint(sourceJoint); BodyPositions[jt] = GetVector3FromJoint(sourceJoint); } _rightHandFilter.Record(_trackedBody.HandRightState == HandState.Closed); _leftHandFilter.Record(_trackedBody.HandLeftState == HandState.Closed); }
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject) { 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 = GetVector3FromJoint(sourceJoint); 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; } } //float shoulderElbowAngleL = GetAngle(body, Kinect.JointType.ShoulderLeft, Kinect.JointType.ElbowLeft); //Debug.Log(shoulderElbowAngleL); //float hipKneeAngleL = GetAngle(body, Kinect.JointType.HipLeft, Kinect.JointType.KneeLeft); //Debug.Log(hipKneeAngleL); float hipKneeAngleR = GetAngle(body, Kinect.JointType.HipRight, Kinect.JointType.KneeRight); Debug.Log(hipKneeAngleR); }
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject) { JointTransforms.Clear(); JointOrientations.Clear(); bodyTracked = body.IsTracked; 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.FindChild(jt.ToString()); // UPDATE STATIC VAR JointTransforms.Add(jointObj); JointOrientations.Add(body.JointOrientations[jt]); jointObj.localPosition = GetVector3FromJoint(sourceJoint); 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; } } }
// return current position of given joint private Vector3 GetVector3FromJoint(Kinect.Joint joint) { // return position with scale to enlarge skeleton return(new Vector3(joint.Position.X * SkeletonScale, joint.Position.Y * SkeletonScale, joint.Position.Z * SkeletonScale)); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { bool bNewFrame = false; if ((multiSourceFrameReader != null && multiSourceFrame != null) || bodyFrameReader != null) { var frame = multiSourceFrame != null?multiSourceFrame.BodyFrameReference.AcquireFrame() : bodyFrameReader.AcquireLatestFrame(); if (frame != null) { frame.GetAndRefreshBodyData(bodyData); bodyFrame.liRelativeTime = frame.RelativeTime.Ticks; frame.Dispose(); frame = null; for (int i = 0; i < sensorData.bodyCount; i++) { Body body = bodyData[i]; if (body == null) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if (body.IsTracked) { // transfer body and joints data //传送身体和关节数据 bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId; for (int j = 0; j < sensorData.jointCount; j++) { Windows.Kinect.Joint joint = body.Joints[(Windows.Kinect.JointType)j]; KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; jointData.jointType = (KinectInterop.JointType)j; jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState; if ((int)joint.TrackingState != (int)TrackingState.NotTracked) { jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.position = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos); } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // jointData.orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } // tranfer hand states //转变手的状态 bodyFrame.bodyData[i].leftHandState = (KinectInterop.HandState)body.HandLeftState; bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence; bodyFrame.bodyData[i].rightHandState = (KinectInterop.HandState)body.HandRightState; bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence; } } bNewFrame = true; } } return(bNewFrame); }
public bool PollBodyFrame(KinectInterop.SensorData sensorData, ref KinectInterop.BodyFrameData bodyFrame, ref Matrix4x4 kinectToWorld) { bool bNewFrame = false; if ((multiSourceFrameReader != null && multiSourceFrame != null) || bodyFrameReader != null) { var frame = multiSourceFrame != null?multiSourceFrame.BodyFrameReference.AcquireFrame() : bodyFrameReader.AcquireLatestFrame(); if (frame != null) { frame.GetAndRefreshBodyData(bodyData); bodyFrame.liRelativeTime = frame.RelativeTime.Ticks; if (sensorData.hintHeightAngle) { // get the floor plane Windows.Kinect.Vector4 vFloorPlane = frame.FloorClipPlane; Vector3 floorPlane = new Vector3(vFloorPlane.X, vFloorPlane.Y, vFloorPlane.Z); sensorData.sensorRotDetected = Quaternion.FromToRotation(floorPlane, Vector3.up); sensorData.sensorHgtDetected = vFloorPlane.W; } frame.Dispose(); frame = null; for (int i = 0; i < sensorData.bodyCount; i++) { Body body = bodyData[i]; if (body == null) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if (body.IsTracked) { // transfer body and joints data bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId; // cache the body joints (following the advice of Brian Chasalow) Dictionary <Windows.Kinect.JointType, Windows.Kinect.Joint> bodyJoints = body.Joints; for (int j = 0; j < sensorData.jointCount; j++) { Windows.Kinect.Joint joint = bodyJoints[(Windows.Kinect.JointType)j]; KinectInterop.JointData jointData = bodyFrame.bodyData[i].joint[j]; jointData.jointType = (KinectInterop.JointType)j; jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState; if ((int)joint.TrackingState != (int)TrackingState.NotTracked) { jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.position = kinectToWorld.MultiplyPoint3x4(jointData.kinectPos); } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // jointData.orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } // tranfer hand states bodyFrame.bodyData[i].leftHandState = (KinectInterop.HandState)body.HandLeftState; bodyFrame.bodyData[i].leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence; bodyFrame.bodyData[i].rightHandState = (KinectInterop.HandState)body.HandRightState; bodyFrame.bodyData[i].rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence; } } bNewFrame = true; } } return(bNewFrame); }
void UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams) { CameraSpacePoint vPrevRawPosition; CameraSpacePoint vPrevFilteredPosition; CameraSpacePoint vPrevTrend; CameraSpacePoint vRawPosition; CameraSpacePoint vFilteredPosition; CameraSpacePoint vPredictedPosition; CameraSpacePoint vDiff; CameraSpacePoint vTrend; float fDiff; bool bJointIsValid; Windows.Kinect.Joint joint = body.Joints[jt]; vRawPosition = joint.Position; vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition; vPrevTrend = m_pHistory[(int)jt].m_vTrend; vPrevRawPosition = m_pHistory[(int)jt].m_vRawPosition; bJointIsValid = JointPositionIsValid(vRawPosition); // If joint is invalid, reset the filter if (!bJointIsValid) { m_pHistory[(int)jt].m_dwFrameCount = 0; } // Initial start values if (m_pHistory[(int)jt].m_dwFrameCount == 0) { vFilteredPosition = vRawPosition; vTrend = CSVectorZero(); m_pHistory[(int)jt].m_dwFrameCount++; } else if (m_pHistory[(int)jt].m_dwFrameCount == 1) { vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f); vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition); vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection)); m_pHistory[(int)jt].m_dwFrameCount++; } else { // First apply jitter filter vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition); fDiff = CSVectorLength(vDiff); if (fDiff <= smoothingParams.fJitterRadius) { vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius), CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius)); } else { vFilteredPosition = vRawPosition; } // Now the double exponential smoothing filter vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing), CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing)); vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition); vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection)); } // Predict into the future to reduce latency vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction)); // Check that we are not too far away from raw data vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition); fDiff = CSVectorLength(vDiff); if (fDiff > smoothingParams.fMaxDeviationRadius) { vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff), CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff)); } // Save the data from this frame m_pHistory[(int)jt].m_vRawPosition = vRawPosition; m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition; m_pHistory[(int)jt].m_vTrend = vTrend; // Output the data m_pFilteredJoints[(int)jt] = vPredictedPosition; }
// Polls for new skeleton data public static bool PollBodyData(SensorData sensorData, ref BodyFrameData bodyFrame) { bool bNewFrame = false; // BodyFrameData tempBodyFrame = new BodyFrameData(); // int hr = GetBodyFrameData(ref tempBodyFrame, true, true); // //// if((hr == 0) && ((sensorData.colorImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastColorFrameTime)) && //// ((sensorData.depthImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastDepthFrameTime))) // { // if(hr == 0 /**&& (bodyFrame.liRelativeTime > lastFrameTime)*/) // { // bodyFrame = tempBodyFrame; // sensorData.lastBodyFrameTime = bodyFrame.liRelativeTime; // bNewFrame = true; // } // } if (sensorData.bodyFrameReader != null) { var frame = sensorData.bodyFrameReader.AcquireLatestFrame(); if (frame != null) { frame.GetAndRefreshBodyData(sensorData.bodyData); bodyFrame.liRelativeTime = frame.RelativeTime; frame.Dispose(); frame = null; for (int i = 0; i < sensorData.bodyCount; i++) { Body body = sensorData.bodyData[i]; if (body == null) { bodyFrame.bodyData[i].bIsTracked = 0; continue; } bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0); if (body.IsTracked) { bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId; for (int j = 0; j < Constants.JointCount; j++) { Windows.Kinect.Joint joint = body.Joints[(JointType)j]; JointData jointData = bodyFrame.bodyData[i].joint[j]; jointData.jointType = (JointType)j; jointData.trackingState = joint.TrackingState; jointData.position = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z); jointData.kinectPos = jointData.position; //Debug.Log(jointData.jointType + " - " + joint.TrackingState + " " + jointData.kinectPos); if (j == 0) { jointData.direction = Vector3.zero; } else { int jParent = (int)GetParentJoint(jointData.jointType); if (jointData.trackingState != TrackingState.NotTracked && bodyFrame.bodyData[i].joint[jParent].trackingState != TrackingState.NotTracked) { jointData.direction = jointData.kinectPos - bodyFrame.bodyData[i].joint[jParent].kinectPos; } } jointData.orientation = Quaternion.identity; // Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation; // Quaternion orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W); // // if(j != 0) // { // Quaternion parentOri = bodyFrame.bodyData[i].joint[jParent].orientation; // jointData.orientation = parentOri * orientation; // } // else // { // jointData.orientation = orientation; // } if (j == 0) { bodyFrame.bodyData[i].position = jointData.position; bodyFrame.bodyData[i].orientation = jointData.orientation; } bodyFrame.bodyData[i].joint[j] = jointData; } bodyFrame.bodyData[i].leftHandState = body.HandLeftState; bodyFrame.bodyData[i].leftHandConfidence = body.HandLeftConfidence; bodyFrame.bodyData[i].rightHandState = body.HandRightState; bodyFrame.bodyData[i].rightHandConfidence = body.HandRightConfidence; } } bNewFrame = true; } } return(bNewFrame); }
public static Vector3 GetJointPosition2D(Windows.Kinect.Joint joint, float z = 0f) { // TODO: do projection space calculations here instead of 10x values return(new Vector3(joint.Position.X, joint.Position.Y * 10, z)); }
private Vector3 GetBoneVector(Joint parentJoint, Joint childJoint) { var parentVector = parentJoint.Position; var childVector = childJoint.Position; var vector = new Vector3(childVector.X - parentVector.X, childVector.Y - parentVector.Y, childVector.Z - parentVector.Z); return vector; }
private static Vector3 GetVector3FromJoint(Kinect2.Joint joint) { return(new Vector3(joint.Position.X, joint.Position.Y + 1.0f, -joint.Position.Z + 2.0f)); }
private static Vector3 GetVector3FromJoint(Windows.Kinect.Joint joint) { return(new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z)); }
public float CalculateDistance(Windows.Kinect.Joint g1, Windows.Kinect.Joint g2) { return(Vector3.Distance(GetVector3FromJoint(g1), GetVector3FromJoint(g2))); }
public bool Equals(Joint obj) { return JointType.Equals(obj.JointType) && Position.Equals(obj.Position) && TrackingState.Equals(obj.TrackingState); }
private void PlayerMovement(Kinect.Joint PlayerPosition) { float MoveBuffer = 2.25f; transform.position = new Vector3(-PlayerPosition.Position.X + InitialPosition.x, InitialPosition.y, (PlayerPosition.Position.Z - MoveBuffer + InitialPosition.z)); }
//Joint should be of JointType "joinTypeToAttachTo" public abstract void UpdatePosition(ulong uniqueId, Kinect.Joint joint, Vector3 jointWorldPosition);
private void UpdateBodyObject(Kinect.Body body, GameObject bodyObject) { if (!sitTest) { for (Kinect.JointType jt = Kinect.JointType.HipLeft; jt <= Kinect.JointType.ThumbRight; jt++) { Kinect.Joint sourceJoint = body.Joints[jt]; Kinect.Joint?targetJoint = null; //IF JOINT IS IN BONEMAP ADD IT TO TARGET JOINT if (_BoneMap.ContainsKey(jt)) { targetJoint = body.Joints[_BoneMap[jt]]; } Transform jointObj = bodyObject.transform.Find(jt.ToString()); jointObj.localPosition = GetVector3FromJoint(sourceJoint); LineRenderer lr = jointObj.GetComponent <LineRenderer>(); if (targetJoint.HasValue) { lr.SetPosition(0, jointObj.localPosition); lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value)); lr.startColor = Color.white; //(Color.grey,Color.grey); lr.endColor = Color.grey; float legAngle = FindHipAngle(GetVector3FromJoint(sourceJoint), GetVector3FromJoint(targetJoint.Value)); //Debug.Log(legAngle); //-5 <= x <= 5 -> tollerence TOTEST if (legAngle >= -5 && legAngle <= 5) { sitTest = true; //ghetto solution -> couldnt get the face to be not visible at start --> set face after sit } } else { lr.enabled = false; } } } if (sitTest) { if (!skelTest) { //iterate over all the joints that arent hands and remover them for (Kinect.JointType jt = Kinect.JointType.HipLeft; jt <= Kinect.JointType.ThumbRight; jt++) { if (jt != Kinect.JointType.HandRight || jt != Kinect.JointType.HandLeft) { GameObject.Find(jt.ToString()).SetActive(false); } } skelTest = true; } //for face and hands (single joints) foreach (Kinect.JointType _joint in _joints) { //get new target position Joint sourceJoint = body.Joints[_joint]; Vector3 targetPosition = GetVector3FromJoint(sourceJoint); //all play happens on same plane targetPosition.z = 0; //get joint, set new postion Transform jointObject = bodyObject.transform.Find(_joint.ToString()); jointObject.position = targetPosition; Joint selectedJoint = body.Joints[_joint]; if (_joint.ToString() != "Head") { if (_joint.ToString() == "HandLeft") { leftHandPos = GetVector3FromJoint(selectedJoint); } else { rightHandPos = GetVector3FromJoint(selectedJoint); } } //0.5 is radius if (leftHandPos.x <= -4.5 && leftHandPos.x >= -5.5) { //Debug.Log(rightHandPos.y); } } } }
private BoneTrackingState GetBoneTrackingState(Joint nearJoint, Joint farJoint) { var nearState = nearJoint.TrackingState; var farState = farJoint.TrackingState; //T + T => T //T + I => I //I + T => I //I + I => I if (nearState == TrackingState.Tracked && farState == TrackingState.Tracked) { return BoneTrackingState.Tracked; } if (nearState == TrackingState.NotTracked || farState == TrackingState.NotTracked) { return BoneTrackingState.NotTracked; } //Can't be both tracked, and neither are not tracked //Must be inferred and either tracking or inferred return BoneTrackingState.Inferred; }
public Vector3 ToVector3(Windows.Kinect.Joint j) => new Vector3(j.Position.X, j.Position.Y, j.Position.Z);
// Update is called once per frame void Update() { if (_BodyManager == null) { Debug.Log("_BodyManager == null"); return; } // Bodyデータを取得する var data = _BodyManager.GetData(); if (data == null) { return; } // 最初に追跡している人を取得する var body = data.FirstOrDefault(b => b.IsTracked); if (body == null) { return; } Kinect.Joint sourceJoint = body.Joints[Kinect.JointType.SpineBase]; Debug.Log("Z:" + sourceJoint.Position.Z); if (sourceJoint.Position.Z <= 0.2 || sourceJoint.Position.Z >= 1.5) { return; } // 床の傾きを取得する var floorPlane = _BodyManager.FloorClipPlane; var comp = Quaternion.FromToRotation( new Vector3(floorPlane.X, floorPlane.Y, floorPlane.Z), Vector3.up); // 関節の回転を取得する var joints = body.JointOrientations; Quaternion SpineBase; Quaternion SpineMid; Quaternion SpineShoulder; Quaternion ShoulderLeft; Quaternion ShoulderRight; Quaternion ElbowLeft; Quaternion WristLeft; Quaternion HandLeft; Quaternion ElbowRight; Quaternion WristRight; Quaternion HandRight; Quaternion KneeLeft; Quaternion AnkleLeft; Quaternion KneeRight; Quaternion AnkleRight; // 鏡 if (IsMirror) { SpineBase = joints[JointType.SpineBase].Orientation.ToMirror().ToQuaternion(comp); SpineMid = joints[JointType.SpineMid].Orientation.ToMirror().ToQuaternion(comp); SpineShoulder = joints[JointType.SpineShoulder].Orientation.ToMirror().ToQuaternion(comp); ShoulderLeft = joints[JointType.ShoulderRight].Orientation.ToMirror().ToQuaternion(comp); ShoulderRight = joints[JointType.ShoulderLeft].Orientation.ToMirror().ToQuaternion(comp); ElbowLeft = joints[JointType.ElbowRight].Orientation.ToMirror().ToQuaternion(comp); WristLeft = joints[JointType.WristRight].Orientation.ToMirror().ToQuaternion(comp); HandLeft = joints[JointType.HandRight].Orientation.ToMirror().ToQuaternion(comp); ElbowRight = joints[JointType.ElbowLeft].Orientation.ToMirror().ToQuaternion(comp); WristRight = joints[JointType.WristLeft].Orientation.ToMirror().ToQuaternion(comp); HandRight = joints[JointType.HandLeft].Orientation.ToMirror().ToQuaternion(comp); KneeLeft = joints[JointType.KneeRight].Orientation.ToMirror().ToQuaternion(comp); AnkleLeft = joints[JointType.AnkleRight].Orientation.ToMirror().ToQuaternion(comp); KneeRight = joints[JointType.KneeLeft].Orientation.ToMirror().ToQuaternion(comp); AnkleRight = joints[JointType.AnkleLeft].Orientation.ToMirror().ToQuaternion(comp); } // そのまま else { SpineBase = joints[JointType.SpineBase].Orientation.ToQuaternion(comp); SpineMid = joints[JointType.SpineMid].Orientation.ToQuaternion(comp); SpineShoulder = joints[JointType.SpineShoulder].Orientation.ToQuaternion(comp); ShoulderLeft = joints[JointType.ShoulderLeft].Orientation.ToQuaternion(comp); ShoulderRight = joints[JointType.ShoulderRight].Orientation.ToQuaternion(comp); ElbowLeft = joints[JointType.ElbowLeft].Orientation.ToQuaternion(comp); WristLeft = joints[JointType.WristLeft].Orientation.ToQuaternion(comp); HandLeft = joints[JointType.HandLeft].Orientation.ToQuaternion(comp); ElbowRight = joints[JointType.ElbowRight].Orientation.ToQuaternion(comp); WristRight = joints[JointType.WristRight].Orientation.ToQuaternion(comp); HandRight = joints[JointType.HandRight].Orientation.ToQuaternion(comp); KneeLeft = joints[JointType.KneeLeft].Orientation.ToQuaternion(comp); AnkleLeft = joints[JointType.AnkleLeft].Orientation.ToQuaternion(comp); KneeRight = joints[JointType.KneeRight].Orientation.ToQuaternion(comp); AnkleRight = joints[JointType.AnkleRight].Orientation.ToQuaternion(comp); } // 関節の回転を計算する var q = Ref.transform.rotation; Ref.transform.rotation = Quaternion.identity; var comp2 = Quaternion.AngleAxis(90, new Vector3(0, 1, 0)) * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1)); Spine1.transform.rotation = SpineMid * comp2; RightArm.transform.rotation = ElbowRight * comp2; RightForeArm.transform.rotation = WristRight * comp2; RightHand.transform.rotation = HandRight * comp2; LeftArm.transform.rotation = ElbowLeft * comp2; LeftForeArm.transform.rotation = WristLeft * comp2; LeftHand.transform.rotation = HandLeft * comp2; RightUpLeg.transform.rotation = KneeRight * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1)); RightLeg.transform.rotation = AnkleRight * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1)); LeftUpLeg.transform.rotation = KneeLeft * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1)); LeftLeg.transform.rotation = AnkleLeft * Quaternion.AngleAxis(-90, new Vector3(0, 0, 1)); // モデルの回転を設定する Ref.transform.rotation = q; // モデルの位置を移動する var pos = body.Joints[JointType.SpineMid].Position; Ref.transform.position = new Vector3(-pos.X, pos.Y, -pos.Z); }
private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject) { int i = 0; for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.HandRight; jt++) { Kinect.Joint sourceJoint = body.Joints[jt]; //size ? Kinect.Joint?targetJoint = null; if (_BoneMap.ContainsKey(jt)) { targetJoint = body.Joints[_BoneMap[jt]]; } Transform jointObj = bodyObject.transform.FindChild(jt.ToString()); if (jt == Kinect.JointType.HandRight) { jointObj.localPosition = GetVector3FromJoint(sourceJoint) + bodyOffset + rightHandOffset; } else { jointObj.localPosition = GetVector3FromJoint(sourceJoint) + bodyOffset; } Vector3 transPosition = GetVector3FromJoint(sourceJoint) + bodyOffset; //’�ˆÓ Debug.Log(transPosition.z); Debug.Log(jointObj.localPosition.z); positionFloats[(i * 3)] = transPosition.x; positionFloats[(i * 3) + 1] = transPosition.y; positionFloats[(i * 3) + 2] = transPosition.z; i++; LineRenderer lr = jointObj.GetComponent <LineRenderer>(); if (targetJoint.HasValue) { lr.SetPosition(0, jointObj.localPosition); if (jt == Kinect.JointType.HandRight) { lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value) + bodyOffset + rightHandOffset); } else { lr.SetPosition(1, GetVector3FromJoint(targetJoint.Value) + bodyOffset); } } else { lr.enabled = false; } } /*for (Kinect.JointType jt = Kinect.JointType.HandRight+1; jt <= Kinect.JointType.ThumbRight; jt++) { * Kinect.Joint sourceJoint = body.Joints[jt]; //size ? * Vector3 transPosition = GetVector3FromJointforSend(sourceJoint); * * positionFloats[(i * 3)] = transPosition.x; * positionFloats[(i * 3) + 1] = transPosition.y; * positionFloats[(i * 3) + 2] = transPosition.z; * * i++; * }*/ if (Input.GetMouseButton(2)) { i = 0; if (Input.GetMouseButtonDown(2)) { streamWriter.Close(); boneNumber++; streamWriter = new StreamWriter("./bone" + boneNumber + ".txt", true); } for (Kinect.JointType jt = Kinect.JointType.SpineBase; jt <= Kinect.JointType.HandRight; jt++) { //Start Record i++; } streamWriter.Write(":"); streamWriter.Flush(); } }
private Vector3 GetVector3FromJoint(Kinect.Joint joint) { //Debug.Log("joint = " + joint.Position.X + " y: "+ joint.Position.Y + " z: " + joint.Position.Z); return(new Vector3(joint.Position.X * boneSize * xTransferAmount + xOffset, joint.Position.Y * boneSize * yTransferAmount + yOffset, joint.Position.Z * boneSize * -1 * zTransferAmount + zOffset)); }
private static Vector3 GetVector3FromJoint(Kinect.Joint joint) { return(new Vector3(joint.Position.X * 10, joint.Position.Y * 10, joint.Position.Z * 10)); }
/// <summary> /// Update the filter for one joint. /// </summary> /// <param name="skeleton">The Skeleton to filter.</param> /// <param name="jt">The Skeleton Joint index to filter.</param> /// <param name="smoothingParameters">The Smoothing parameters to apply.</param> protected void FilterJoint(ref Kinect.Body skeleton, Kinect.JointType jt, TransformSmoothParameters smoothingParameters) { if (null == skeleton) { return; } // Debug.Log("here"); int jointIndex = (int)jt; Vector3 filteredPosition; Vector3 diffvec; Vector3 trend; float diffVal; Vector3 rawPosition = GetVector3FromJoint(skeleton.Joints[jt]); Vector3 prevFilteredPosition = this.history[jointIndex].FilteredPosition; Vector3 prevTrend = this.history[jointIndex].Trend; Vector3 prevRawPosition = this.history[jointIndex].RawPosition; bool jointIsValid = JointPositionIsValid(rawPosition); /*if(jt == Kinect.JointType.HandLeft) Debug.Log ("raw " +":"+ rawPosition.x + ", " + rawPosition.y + ", " + rawPosition.z); */ // If joint is invalid, reset the filter if (!jointIsValid) { this.history[jointIndex].FrameCount = 0; } // Initial start values if (this.history[jointIndex].FrameCount == 0) { filteredPosition = rawPosition; trend = Vector3.zero; // if(jt == Kinect.JointType.HandLeft) // Debug.Log ("filtered pos" +":"+ filteredPosition.x + ", " + filteredPosition.z + ", " + filteredPosition.z); } else if (this.history[jointIndex].FrameCount == 1) { filteredPosition = (rawPosition+ prevRawPosition) * 0.5f; diffvec = filteredPosition - prevFilteredPosition; trend = (diffvec * smoothingParameters.Correction) + (prevTrend * (1.0f - smoothingParameters.Correction)); } else { // First apply jitter filter diffvec = rawPosition - prevFilteredPosition; diffVal = Mathf.Abs(diffvec.magnitude); if (diffVal <= smoothingParameters.JitterRadius) { filteredPosition = (rawPosition * (diffVal / smoothingParameters.JitterRadius)) + (prevFilteredPosition * ( 1.0f - (diffVal / smoothingParameters.JitterRadius))); } else { filteredPosition = rawPosition; } // Now the double exponential smoothing filter filteredPosition = (filteredPosition *(1.0f - smoothingParameters.Smoothing)) + ((prevFilteredPosition + prevTrend) * smoothingParameters.Smoothing); // if(jt == Kinect.JointType.HandLeft) // Debug.Log ("filtered pos" +":"+ filteredPosition.x + ", " + filteredPosition.z + ", " + filteredPosition.z); diffvec = filteredPosition - prevFilteredPosition; trend = (diffvec * smoothingParameters.Correction) + (prevTrend *( 1.0f - smoothingParameters.Correction)); } // Predict into the future to reduce latency Vector3 predictedPosition = filteredPosition + (trend *smoothingParameters.Prediction); // Check that we are not too far away from raw data diffvec = predictedPosition- rawPosition; diffVal = Mathf.Abs(diffvec.magnitude); if (diffVal > smoothingParameters.MaxDeviationRadius) { predictedPosition = (predictedPosition * (smoothingParameters.MaxDeviationRadius / diffVal)) + (rawPosition* ( 1.0f - (smoothingParameters.MaxDeviationRadius / diffVal))); } // Save the data from this frame this.history[jointIndex].RawPosition = rawPosition; this.history[jointIndex].FilteredPosition = filteredPosition; this.history[jointIndex].Trend = trend; this.history[jointIndex].FrameCount++; //Debug.Log (this.history[jointIndex].FrameCount); // Set the filtered data back into the joint Kinect.Joint j = new Kinect.Joint(); j.Position = Vector3ToSkeletonPoint(predictedPosition); //if(skeleton.Joints.ContainsKey(jt)) //skeleton.Joints.Remove(jt); FilteredJoints[jt] = j;// =Vector3ToSkeletonPoint(predictedPosition);// j; /* if (jt == Kinect.JointType.HandLeft) { Debug.Log ("predicted " + ":" + j.Position.X + ", " + j.Position.Y + ", " + j.Position.Z); //Debug.Log ("new " + ":" + skeleton.Joints[jt].Position.X + ", " + skeleton.Joints[jt].Position.Y + ", " + skeleton.Joints[jt].Position.Z); }*/ }