void TransformBone(uint userId, SkeletonJoint joint, Transform dest, bool move)
{
SkeletonJointPosition sjp = this.joints[userId][joint];
Point3D pos = sjp.position;
SkeletonJointOrientation ori = new SkeletonJointOrientation();
this.skeletonCapbility.GetSkeletonJointOrientation(userId, joint, ori);
float [] m = ori.Orientation.elements;
// only modify joint if confidence is high enough in this frame
if (ori.Confidence > 0.5)
{
// Z coordinate in OpenNI is opposite from Unity. We will create a quat
// to rotate from OpenNI to Unity (relative to initial rotation)
// Vector3 worldZVec = new Vector3(-ori.m02, -ori.m12, ori.m22);
Vector3 worldZVec = new Vector3(-m[2], -m[5], m[8]);
// Vector3 worldYVec = new Vector3(trans.ori.m01, trans.ori.m11, -trans.ori.m21);
Vector3 worldYVec = new Vector3(m[1], m[4], -m[7]);
Quaternion jointRotation = Quaternion.LookRotation(worldZVec, worldYVec);
Quaternion newRotation = jointRotation * initialRotations[(int)joint];
// Some smoothing
dest.rotation = Quaternion.Slerp(dest.rotation, newRotation, Time.deltaTime * 20);
}
if (move)
{
// dest.position = new Vector3(trans.pos.x/1000, trans.pos.y/1000 -1, -trans.pos.z/1000);
dest.position = new Vector3(pos.X / 1000, pos.Y / 1000 - 1, -pos.Z / 1000);
}
}
public void JointFromJSON(SkeletonJoint j, Hashtable dict)
{
ArrayList positionList = (ArrayList)dict["Position"];
ArrayList orientationList = (ArrayList)dict["Orientation"];
SkeletonJointOrientation sjo = new SkeletonJointOrientation();
sjo.X1 = 1.0f;
SkeletonJointPosition sjp = new SkeletonJointPosition();
SkeletonJointTransformation xform = new SkeletonJointTransformation();
// object -> double ->float is okay, but object->float isn't
// (the object is a Double)
sjp.Position = new Point3D((float)(double)positionList[0],
(float)(double)positionList[1],
(float)(double)positionList[2]);
sjo.X1 = (float)(double)orientationList[0];
sjo.X2 = (float)(double)orientationList[1];
sjo.X3 = (float)(double)orientationList[2];
sjo.Y1 = (float)(double)orientationList[3];
sjo.Y2 = (float)(double)orientationList[4];
sjo.Y3 = (float)(double)orientationList[5];
sjo.Z1 = (float)(double)orientationList[6];
sjo.Z2 = (float)(double)orientationList[7];
sjo.Z3 = (float)(double)orientationList[8];
xform.Orientation = sjo;
xform.Position = sjp;
UpdateJoint(j, xform);
}
public SkeletonJointOrientation GetSkeletonJointOrientation(UserID user, SkeletonJoint joint)
{
SkeletonJointOrientation orientation = new SkeletonJointOrientation();
int status = SafeNativeMethods.xnGetSkeletonJointOrientation(this.InternalObject, user, joint, ref orientation);
WrapperUtils.ThrowOnError(status);
return(orientation);
}
public static bool GetJointOrientation(uint userID, int joint, ref SkeletonJointOrientation pOrientation)
{
if (GetJointOrientationNative(userID, joint, ref pOrientation))
{
return(true);
}
return(false);
}
/// initialize from OpenNI data
public void FromRaw(SkeletonJointTransformation k)
{
// m_joint = joint;
Point3D p = k.Position.Position;
m_pos.x = p.X;
m_pos.y = p.Y;
m_pos.z = 1500-p.Z; // the z axis of the (Kinect) sensor is opposite to the one in Unity3D.
// 1500 is a arbitrary offset to make the value around 0.
m_posConf = k.Position.Confidence;
SkeletonJointOrientation o = k.Orientation;
m_orient = new Vector3[3];
m_orient[0].x = o.X1; m_orient[0].y = o.X2; m_orient[0].z = -o.X3; // the z axis of the sensor is opposite to the one in Unity3D
m_orient[1].x = o.Y1; m_orient[1].y = o.Y2; m_orient[1].z = -o.Y3;
m_orient[2].x = o.Z1; m_orient[2].y = o.Z2; m_orient[2].z = -o.Z3;
m_orientConf = o.Confidence;
}
void TransformBone(uint userId, SkeletonJoint joint, Transform dest, bool move)
{
// SkeletonJointPosition sjp = this.joints[userId][joint];
// Point3D pos = sjp.position;
//Debug.Log("joint " + joint + "x " + pos.X + " y " + pos.Y + " z " + pos.Z);
SkeletonJointOrientation ori = new SkeletonJointOrientation();
this.skeletonCapbility.GetSkeletonJointOrientation(userId, joint, ref ori);
//float [] m = ori.Orientation.elements;
//Debug.Log(m.Length);
// only modify joint if confidence is high enough in this frame
if (ori.Confidence > 0.5)
{
// Z coordinate in OpenNI is opposite from Unity. We will create a quat
// to rotate from OpenNI to Unity (relative to initial rotation)
// Vector3 worldZVec = new Vector3(-ori.m02, -ori.m12, ori.m22);
// elem1 elem2 elem3
// elem4 elem5 elem6
// elem7 elem8 elem9
Vector3 worldZVec = new Vector3(-ori.Orientation.elem3, -ori.Orientation.elem6, ori.Orientation.elem9);
// Vector3 worldYVec = new Vector3(trans.ori.m01, trans.ori.m11, -trans.ori.m21);
Vector3 worldYVec = new Vector3(ori.Orientation.elem2, ori.Orientation.elem5, -ori.Orientation.elem8);
Quaternion jointRotation = Quaternion.LookRotation(worldZVec, worldYVec);
Quaternion newRotation = (jointRotation * initialRotations[(int)joint]);
// Some smoothing
dest.rotation = Quaternion.Slerp(dest.rotation, newRotation, Time.deltaTime * 20);
}
//if (move)
//{
// dest.position = new Vector3(trans.pos.x/1000, trans.pos.y/1000 -1, -trans.pos.z/1000);
// dest.position = new Vector3(pos.X/1000, pos.Y/1000 -1, -pos.Z/1000);
//}
}
public static Quaternion SkeletonJointOrientationToQuaternion(SkeletonJointOrientation m)
{
float tr = m.X1 + m.Y2 + m.Z3;
float S = 0f;
float qw = 0f;
float qx = 0f;
float qy = 0f;
float qz = 0f;
if (tr > 0)
{
S = Mathf.Sqrt(tr + 1.0f) * 2f;
qw = 0.25f * S;
qx = (m.Y3 - m.Z2) / S;
qy = (m.Z1 - m.X3) / S;
qz = (m.X2 - m.Y1) / S;
}
else if ((m.X1 > m.Y2) && (m.X1 > m.Z3))
{
S = Mathf.Sqrt(1.0f + m.X1 - m.Y2 - m.Z3) * 2f;
qw = (m.Y3 - m.Z2) / S;
qx = 0.25f * S;
qy = (m.Y1 + m.X2) / S;
qz = (m.Z1 + m.X3) / S;
}
else if (m.Y2 > m.Z3)
{
S = Mathf.Sqrt(1.0f + m.Y2 - m.X1 - m.Z3) * 2f;
qw = (m.Z1 - m.X3) / S;
qx = (m.Y1 + m.X2) / S;
qy = 0.25f * S;
qz = (m.Z2 + m.Y3) / S;
}
else
{
S = Mathf.Sqrt(1.0f + m.Z3 - m.X1 - m.Y2) * 2f;
qw = (m.X2 - m.Y1) / S;
qx = (m.Z1 + m.X3) / S;
qy = (m.Z2 + m.Y3) / S;
qz = 0.25f * S;
}
return(new Quaternion(qx, qy, qz, qw));
}
/// @brief Gets the current joint orientation for a specific joint
///
/// @param joint The joint we want the orientation to.
/// @param curRotation [out] The current joint rotation
/// @return True on success and false on failure (e.g. the user is not tracking).
/// @note An exception might occur if there is an error (e.g. an illegal joint is used).
public bool GetSkeletonJointOrientation(SkeletonJoint joint, out SkeletonJointOrientation curRotation)
{
curRotation = NIPlayerCandidateObject.m_InitializedZero.Orientation;
if (!Tracking)
{
return(false);
}
if (m_user.Skeleton == null)
{
return(false);
}
try
{
curRotation = m_user.Skeleton.GetSkeletonJointOrientation(m_user.OpenNIUserID, joint);
}
catch
{
return(false);
}
return(true);
}
public void DrawOrientation(ref WriteableBitmap image, int id, UserGenerator userGenerator, SkeletonJoint joint, Point3D corner)
{
SkeletonJointOrientation orientation = new SkeletonJointOrientation();
SkeletonJointPosition position = new SkeletonJointPosition();
position = userGenerator.SkeletonCapability.GetSkeletonJointPosition(id, joint);
orientation = userGenerator.SkeletonCapability.GetSkeletonJointOrientation(id, joint);
if (position.Confidence != 1 && orientation.Confidence != 1)
{
return;
}
SkeletonJointPosition v1 = new SkeletonJointPosition();
SkeletonJointPosition v2 = new SkeletonJointPosition();
v1.Confidence = v2.Confidence = 1;
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X1,
v1.Position.Y + 100 * orientation.Y1,
v1.Position.Z + 100 * orientation.Z1);
DrawTheLine(ref image, ref v1, ref v2);
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X2,
v1.Position.Y + 100 * orientation.Y2,
v1.Position.Z + 100 * orientation.Z2);
DrawTheLine(ref image, ref v1, ref v2);
v1.Position = position.Position;
v2.Position = new Point3D(v1.Position.X + 100 * orientation.X3,
v1.Position.Y + 100 * orientation.Y3,
v1.Position.Z + 100 * orientation.Z3);
DrawTheLine(ref image, ref v1, ref v2);
}
// Update is called once per frame
void Update()
{
//Debug.Log("Update");
if (this.shouldRun)
{
try{
this.context.WaitOneUpdateAll(this.depth);
}catch (Exception) {
Debug.Log("No paso");
}
//Debug.Log("H "+Input.GetAxis("Horizontal"));
//Debug.Log("V "+Input.GetAxis("Vertical"));
int[] users = this.userGenerator.GetUsers();
foreach (int user in users)
{
if (this.skeletonCapability.IsTracking(user))
{
//Debug.Log ("Esta trackeando Usuario # "+user);
//obtener la orientacion del joint
SkeletonJointOrientation ori = this.skeletonCapability.GetSkeletonJointOrientation(user, SkeletonJoint.Torso);
/*
* Debug.Log("X1= "+ori.X1);
* Debug.Log("X2= "+ori.X2);
* Debug.Log("X3= "+ori.X3);
* Debug.Log("Y1= "+ori.Y1);
* Debug.Log("Y2= "+ori.Y2);
* Debug.Log("Y3= "+ori.Y3);
* Debug.Log("Z1= "+ori.Z1);
* Debug.Log("Z2= "+ori.Z2);
* Debug.Log("Z3= "+ori.Z3);
*/
Quaternion q = SkeletonJointOrientationToQuaternion(ori);
//Debug.Log(q.y);
if (q.y > .30)
{
transform.Rotate(new Vector3(0f, valorPositivoRotacion, 0f));
}
else if (q.y < -.30)
{
transform.Rotate(new Vector3(0f, -valorNegativoRotacion, 0f));
}
if (q.x > .20)
{
if (transform.rotation.x < .25)
{
transform.Rotate(new Vector3(valorPositivoRotacion, 0f, 0f));
}
}
else if (q.x < -.20)
{
if (transform.rotation.x > -.25)
{
transform.Rotate(new Vector3(-valorNegativoRotacion, 0f, 0f));
}
}
else
{
Quaternion fromX = new Quaternion(transform.rotation.x, transform.rotation.y, 0f, transform.rotation.w);
Quaternion toX = new Quaternion(0f, transform.rotation.y, 0f, transform.rotation.w);
transform.rotation = Quaternion.Lerp(fromX, toX, Time.time * .001f);
//transform.Rotate (new Vector3((transform.rotation.x *-1),0f,0f));
}
}
}
}
}
public static bool GetSkeletonJointOrientation(uint userID, int joint, bool flip, ref Quaternion pJointRot)
{
Matrix4x4 matOri = Matrix4x4.identity;
bool bMatHasOri = false;
if (joint == (int)SkeletonJoint.LEFT_HAND)
{
// special case - left hand
SkeletonJointPosition posElbow = new SkeletonJointPosition();
SkeletonJointPosition posHand = new SkeletonJointPosition();
SkeletonJointPosition posHips = new SkeletonJointPosition();
SkeletonJointPosition posNeck = new SkeletonJointPosition();
bool bElbowTracked = GetJointPosition(userID, (int)SkeletonJoint.LEFT_ELBOW, ref posElbow);
bool bHandTracked = GetJointPosition(userID, (int)SkeletonJoint.LEFT_HAND, ref posHand);
bool bHipsTracked = GetJointPosition(userID, (int)SkeletonJoint.HIPS, ref posHips);
bool bNeckTracked = GetJointPosition(userID, (int)SkeletonJoint.NECK, ref posNeck);
if (bElbowTracked && bHandTracked && bHipsTracked && bNeckTracked)
{
Vector3 vElbow = new Vector3(posElbow.x, posElbow.y, posElbow.z);
Vector3 vHand = new Vector3(posHand.x, posHand.y, posHand.z);
Vector3 vHips = new Vector3(posHips.x, posHips.y, posHips.z);
Vector3 vNeck = new Vector3(posNeck.x, posNeck.y, posNeck.z);
Vector3 vx = -(vHand - vElbow);
Vector3 vy = vNeck - vHips;
MakeMatrixFromXY(vx, vy, ref matOri);
bMatHasOri = true;
}
}
else if (joint == (int)SkeletonJoint.RIGHT_HAND)
{
// special case - right hand
SkeletonJointPosition posElbow = new SkeletonJointPosition();
SkeletonJointPosition posHand = new SkeletonJointPosition();
SkeletonJointPosition posHips = new SkeletonJointPosition();
SkeletonJointPosition posNeck = new SkeletonJointPosition();
bool bElbowTracked = GetJointPosition(userID, (int)SkeletonJoint.RIGHT_ELBOW, ref posElbow);
bool bHandTracked = GetJointPosition(userID, (int)SkeletonJoint.RIGHT_HAND, ref posHand);
bool bHipsTracked = GetJointPosition(userID, (int)SkeletonJoint.HIPS, ref posHips);
bool bNeckTracked = GetJointPosition(userID, (int)SkeletonJoint.NECK, ref posNeck);
if (bElbowTracked && bHandTracked && bHipsTracked && bNeckTracked)
{
Vector3 vElbow = new Vector3(posElbow.x, posElbow.y, posElbow.z);
Vector3 vHand = new Vector3(posHand.x, posHand.y, posHand.z);
Vector3 vHips = new Vector3(posHips.x, posHips.y, posHips.z);
Vector3 vNeck = new Vector3(posNeck.x, posNeck.y, posNeck.z);
Vector3 vx = vHand - vElbow;
Vector3 vy = vNeck - vHips;
MakeMatrixFromXY(vx, vy, ref matOri);
bMatHasOri = true;
}
}
else
{
// all other joints
SkeletonJointOrientation oriJoint = new SkeletonJointOrientation();
if (GetJointOrientation(userID, joint, ref oriJoint))
{
Quaternion rotJoint = new Quaternion(oriJoint.x, oriJoint.y, oriJoint.z, oriJoint.w);
matOri.SetTRS(Vector3.zero, rotJoint, Vector3.one);
bMatHasOri = true;
}
}
if (bMatHasOri)
{
Vector4 vZ = matOri.GetColumn(2);
Vector4 vY = matOri.GetColumn(1);
if (!flip)
{
vZ.y = -vZ.y;
vY.x = -vY.x;
vY.z = -vY.z;
}
else
{
vZ.x = -vZ.x;
vZ.y = -vZ.y;
vY.z = -vY.z;
}
if (vZ.x != 0.0f || vZ.y != 0.0f || vZ.z != 0.0f)
{
pJointRot = Quaternion.LookRotation(vZ, vY);
}
else
{
bMatHasOri = false;
}
}
return(bMatHasOri);
}
public static extern bool GetJointOrientationNative(uint userID, int joint, ref SkeletonJointOrientation pOrientation);
void Update()
{
//Debug.Log("Update");
if (this.shouldRun)
{
try{
this.context.WaitOneUpdateAll(this.depth);
}catch (Exception) {
Debug.Log("No paso");
}
//Debug.Log("H "+Input.GetAxis("Horizontal"));
//Debug.Log("V "+Input.GetAxis("Vertical"));
int[] users = this.userGenerator.GetUsers();
foreach (int user in users)
{
if (this.skeletonCapability.IsTracking(user))
{
//Debug.Log ("Esta trackeando Usuario # "+user);
//obtener la orientacion del joint
SkeletonJointOrientation ori = this.skeletonCapability.GetSkeletonJointOrientation(user, SkeletonJoint.Torso);
Quaternion q = SkeletonJointOrientationToQuaternion(ori);
//Debug.Log(q.y);
if (q.y > .30)
{
transform.Rotate(new Vector3(0f, valorPositivoRotacion, 0f));
}
else if (q.y < -.30)
{
transform.Rotate(new Vector3(0f, -valorNegativoRotacion, 0f));
}
if (q.x > .20)
{
if (GameObject.Find("CuboCamara").transform.rotation.x < .25)
{
GameObject.Find("CuboCamara").transform.Rotate(new Vector3(valorPositivoRotacion, 0f, 0f));
}
}
else if (q.x < -.20)
{
if (GameObject.Find("CuboCamara").transform.rotation.x > -.25)
{
GameObject.Find("CuboCamara").transform.Rotate(new Vector3(-valorNegativoRotacion, 0f, 0f));
}
}
else
{
Quaternion fromX = new Quaternion(GameObject.Find("CuboCamara").transform.rotation.x, 0f, 0f, GameObject.Find("CuboCamara").transform.rotation.w);
Quaternion toX = new Quaternion(0f, GameObject.Find("CuboCamara").transform.rotation.y, 0f, transform.rotation.w);
//GameObject.Find("CuboCamara").transform.Rotate(Quaternion.Lerp(fromX, toX, Time.time * .001f));
//GameObject.Find("CuboCamara").transform.Rotate(Vector3((GameObject.Find("CuboCamara").rotation.x *-1)*2,0f,0f));
}
//transform.rotation=q;
//Traslacion
SkeletonJointPosition posicion = this.skeletonCapability.GetSkeletonJointPosition(user, SkeletonJoint.Torso);
if (posicion.Position.Z < 1170F)
{
//transform.Translate(Vector3.forward*10f*Time.deltaTime);
transform.Translate(Vector3.forward * 0.107832f * 5 * Time.deltaTime);
}
else if (posicion.Position.Z > 1470F)
{
//transform.Translate(Vector3.back*10f*Time.deltaTime);
transform.Translate(Vector3.back * 0.107832f * 5 * Time.deltaTime);
}
}
}
}
}
public void GetSkeletonJointOrientation(UserID user, SkeletonJoint eJoint, SkeletonJointOrientation joint)
{
UInt32 status = OpenNIImporter.xnGetSkeletonJointOrientation(this.InternalObject, user, eJoint, joint);
WrapperUtils.CheckStatus(status);
}
