public BodyPointPosition ReturnPosition(BodyParts joint)
{
BodyPointPosition pos = new BodyPointPosition();
pos.name = joint;
if (currentBody == null)
{
pos.x = 0;
pos.y = 0;
pos.z = 0;
}
else
{
Windows.Kinect.Joint sourceJoint = currentBody.Joints[bodyPartsTranslation[joint]];
UnityEngine.Vector4 coord = new UnityEngine.Vector4(sourceJoint.Position.X, sourceJoint.Position.Y, sourceJoint.Position.Z, 1.0f);
UnityEngine.Vector4 newCoord = rotMatrix.inverse * coord;
if (joint == BodyParts.ElbowLeft)
{
if (oldText)
{
oldText.text = ("Posicion Old " + joint + ": " + sourceJoint.Position.X + " " + sourceJoint.Position.Y + " " + sourceJoint.Position.Z + " ");
}
}
pos.x = newCoord.x;
pos.y = newCoord.y + floor.w;
pos.z = newCoord.z;
}
return(pos);
}
// Update is called once per frame
void Update()
{
if (GameObject.Find("KinectTAdapter").GetComponent <KinectTwoAdapter>().currentBody != null)
{
if (FullBodyObject == null)
{
return;
}
_FullBodyObject = FullBodyObject.GetComponent <FullBody>();
if (_FullBodyObject == null)
{
return;
}
BodyPointPosition pos = _FullBodyObject.ReturnPointPosition((BodyParts)Enum.Parse(typeof(BodyParts), gameObject.transform.name.ToString()));
Vector3 B = new Vector3(pos.x, pos.y, pos.z);
Vector3 C = B.normalized * StartPosition.magnitude;
Debug.Log("magnitud b " + StartPosition.magnitude + " magnitude c " + C.magnitude);
//float factor = B.magnitude / StartPosition.magnitude;
//B.Scale(new Vector3(factor, factor, factor));
transform.position = C;
//StartPosition = v;
//Debug.Log("Elbow: x:"+pos.x + ", " + pos.y + "" + pos.z);
}
}
public double hipLeftAdMovement()
{
BodyPointPosition pointOne = bodyPointsCollection[BodyParts.HipRight].getCurrentPosition();
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.HipLeft].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.KneeLeft].getCurrentPosition();
Movement objMove = new Movement();
return(Math.Abs(90 - objMove.getAngleJoints(pointOne, pointCenter, pointTwo)));
}
public double headLateralAngle()
{
// lateral flexion
BodyPointPosition pointOne = bodyPointsCollection[BodyParts.SpineShoulder].getCurrentPosition();
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.Neck].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.Head].getCurrentPosition();
Movement objMove = new Movement();
return(180 - objMove.getAngleJoints(pointOne, pointCenter, pointTwo));
}
public double kneeLeftMovement()
{
//Flexion
BodyPointPosition pointOne = bodyPointsCollection[BodyParts.HipLeft].getCurrentPosition();
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.KneeLeft].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.AnkleLeft].getCurrentPosition();
Movement objMove = new Movement();
return(180 - objMove.getAngleJoints(pointOne, pointCenter, pointTwo));
}
void UpdateBodyPoint(BodyParts joint)
{
BodyPointPosition position = _KinectTAdapter.ReturnPosition(joint);
if (joint == BodyParts.FootRight)
{
//infoText.text =("Posicion New "+position.name+": " + position.x + " " + position.y + " " + position.z + " ");
}
bodyPointsCollection[joint].setPosition(position);
}
public double shoulderRotIntRigthMovements()
{
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.ElbowRight].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.WristRight].getCurrentPosition();
BodyPointPosition pointOne = pointCenter;
pointOne.z = pointOne.z - 0.2f;
Movement objMove = new Movement();
return(objMove.getAngleJoints(pointOne, pointCenter, pointTwo));
}
public double hipLeftRotExtMovement()
{
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.KneeLeft].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.AnkleLeft].getCurrentPosition();
BodyPointPosition pointOne = pointCenter;
pointOne.y = pointOne.y - 0.2f;
Movement objMove = new Movement();
return(objMove.getAngleJoints(pointOne, pointCenter, pointTwo));
}
public double elbowFleExtLeftMovement()
{
// Flexion
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.ElbowLeft].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.WristLeft].getCurrentPosition();
BodyPointPosition pointOne = bodyPointsCollection[BodyParts.ShoulderLeft].getCurrentPosition();
Movement objMove = new Movement();
return(180 - objMove.getAngleJoints(pointOne, pointCenter, pointTwo));
}
public double shoulderExtLeftMovements()
{
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.ShoulderLeft].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.ElbowLeft].getCurrentPosition();
BodyPointPosition pointOne = pointCenter;
pointOne.y = pointOne.y - 0.2f;
Movement objMove = new Movement();
return(objMove.getAngleJoints(pointOne, pointCenter, pointTwo));
}
public double hipRigthExtMovement()
{
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.HipRight].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.KneeRight].getCurrentPosition();
BodyPointPosition pointOne = pointCenter;
pointOne.y = pointOne.y + 0.2f;
Movement objMove = new Movement();
return(180 - objMove.getAngleJoints(pointOne, pointCenter, pointTwo));
}
public double spineIncMovement()
{
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.SpineBase].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.SpineShoulder].getCurrentPosition();
BodyPointPosition pointOne = pointCenter;
pointOne.y = pointOne.y - 0.2f;
Movement objMove = new Movement();
return(180 - objMove.getAngleJoints(pointOne, pointCenter, pointTwo));
}
public double spineLatMovement_Abs()
{
BodyPointPosition pointCenter = bodyPointsCollection[BodyParts.SpineBase].getCurrentPosition();
BodyPointPosition pointTwo = bodyPointsCollection[BodyParts.SpineShoulder].getCurrentPosition();
BodyPointPosition pointOne = pointCenter;
pointOne.x = pointOne.x + 0.2f;
Movement objMove = new Movement();
return(Math.Abs(90 - objMove.getAngleJoints(pointOne, pointCenter, pointTwo)));
}
//Get angle of two vectors
public double getAngleJoints(BodyPointPosition pointOne, BodyPointPosition pointCenter, BodyPointPosition pointTwo)
{
BodyPointPosition positionOne = pointOne;
//cout << positionOne.x << ", " << positionOne.y << ", " << positionOne.z << endl;
BodyPointPosition positionCenter = pointCenter;
//cout << positionCenter.x << ", " << positionCenter.y << ", " << positionCenter.z << endl;
BodyPointPosition positionTwo = pointTwo;
//cout << positionTwo.x << ", " << positionTwo.y << ", " << positionTwo.z << endl;
double[] vecAB = { positionOne.x - positionCenter.x, positionOne.y - positionCenter.y, positionOne.z - positionCenter.z };
double[] vecBC = { positionTwo.x - positionCenter.x, positionTwo.y - positionCenter.y, positionTwo.z - positionCenter.z };
double magAB = Math.Sqrt(vecAB[0] * vecAB[0] + vecAB[1] * vecAB[1] + vecAB[2] * vecAB[2]);
double magBC = Math.Sqrt(vecBC[0] * vecBC[0] + vecBC[1] * vecBC[1] + vecBC[2] * vecBC[2]);
double[] vecNormAB = { vecAB[0] / magAB, vecAB[1] / magAB, vecAB[2] / magAB };
double[] vecNormBC = { vecBC[0] / magBC, vecBC[1] / magBC, vecBC[2] / magBC };
double producto = vecNormAB[0] * vecNormBC[0] + vecNormAB[1] * vecNormBC[1] + vecNormAB[2] * vecNormBC[2];
double angulo = Math.Acos(producto) * 180.0f / (Math.PI);
return(angulo);
}
public void setPosition(BodyPointPosition position)
{
currentPosition = position;
}
