private void RefreshModelPositions(Avatar.Body body)
{
Vector3[] meshTempJointLocations = new Vector3[25];
foreach (JointType jt in jointTypes)
{
if (jt.Equals(JointType.SpineShoulder))
{
meshActiveJoints[(int)jt].transform.position = ScalePosition(body.Joints[jt].Position);
continue;
}
if (jt.Equals(JointType.Neck) || jt.Equals(JointType.ShoulderRight) || jt.Equals(JointType.ShoulderLeft) || jt.Equals(JointType.SpineMid))
{
meshTempJointLocations[(int)jt] = ScalePosition(body.Joints[jt].Position);
}
else
{
meshTempJointLocations[(int)jt] = ScalePosition(body.Joints[jt].Position) +
meshActiveJoints[(int)jointParents[jt]].transform.position - meshTempJointLocations[(int)jointParents[jt]];
}
Vector3 parenToChildVector = meshTempJointLocations[(int)jt] - meshActiveJoints[(int)jointParents[jt]].transform.position;
meshActiveJoints[(int)jt].transform.position = meshActiveJoints[(int)jointParents[jt]].transform.position +
parenToChildVector * ((meshInitialJointLocations[jt] - meshInitialJointLocations[jointParents[jt]]).magnitude / parenToChildVector.magnitude);
}
}
public void RefreshSkeletonData(Avatar.Body body)
{
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
Avatar.Joint srcJoint = body.Joints[jt];
Avatar.Joint tgtJoint = null;
if (_BoneMap.ContainsKey(jt))
{
tgtJoint = body.Joints[_BoneMap[jt]];
}
Transform jointObj = this.transform.Find(jt.ToString());
jointObj.localPosition = ScalePosition(srcJoint.Position);
LineRenderer lr = jointObj.GetComponent <LineRenderer>();
if (tgtJoint != null)
{
lr.SetPosition(0, jointObj.localPosition);
lr.SetPosition(1, ScalePosition(tgtJoint.Position));
lr.startColor = GetColorForState(srcJoint.TrackingState);
lr.endColor = GetColorForState(tgtJoint.TrackingState);
}
else
{
lr.enabled = false;
}
}
}
public override Avatar.Body GetFilteredData(Avatar.Body body)
{
AddToSamples(body);
if (MaxSamples <= 1 || BodySamples.Count != MaxSamples)
{
return(BodySamples[0]);
}
return(GetMovingMedianData());
}
public void RefreshAvatarBodyData(Avatar.Body bodyData)
{
Avatar.Body filteredBody = filter.GetFilteredData(bodyData);
if (skeletonIsRendered)
{
SkeletonCont.RefreshSkeletonData(filteredBody);
}
if (modelIsRendered)
{
ModelCont.RefreshModelData(filteredBody);
}
UpdateAvatarSignPosition();
}
void Awake()
{
_Sensor = KinectSensor.GetDefault();
if (_Sensor != null)
{
_Reader = _Sensor.BodyFrameSource.OpenReader();
if (!_Sensor.IsOpen)
{
_Sensor.Open();
}
}
bodyCount = _Sensor.BodyFrameSource.BodyCount;
FaceFrameFeatures faceFrameFeatures = FaceFrameFeatures.RotationOrientation;
faceFrameSources = new FaceFrameSource[bodyCount];
faceFrameReaders = new FaceFrameReader[bodyCount];
avatarBodies = new Avatar.Body[bodyCount];
for (int i = 0; i < bodyCount; i++)
{
faceFrameSources[i] = FaceFrameSource.Create(_Sensor, 0, faceFrameFeatures);
faceFrameReaders[i] = faceFrameSources[i].OpenReader();
}
for (int i = 0; i < bodyCount; i++)
{
avatarBodies[i] = new Avatar.Body();
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
avatarBodies[i].Joints[jt] = new Avatar.Joint();
avatarBodies[i].Joints[jt].JointType = jt;
}
}
}
public void RefreshModelData(Avatar.Body body)
{
RefreshModelPositions(body);
Vector3 quaternionEuler;
quaternionEuler = body.Joints[JointType.KneeRight].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.HipRight].transform.rotation = Quaternion.Euler(hipRightRotX, hipRightRotY, hipRightRotZ) *
Quaternion.identity *
Quaternion.AngleAxis(quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(quaternionEuler.x, Vector3.forward) *
Quaternion.AngleAxis(-quaternionEuler.z, Vector3.right);
quaternionEuler = body.Joints[JointType.AnkleRight].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.KneeRight].transform.rotation = Quaternion.Euler(kneeRightRotX, kneeRightRotY, kneeRightRotZ) *
Quaternion.identity *
Quaternion.AngleAxis(quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(quaternionEuler.x, Vector3.forward) *
Quaternion.AngleAxis(-quaternionEuler.z, Vector3.right);
quaternionEuler = body.Joints[JointType.KneeLeft].Rotation.eulerAngles;;
meshActiveJoints[(int)JointType.HipLeft].transform.rotation = Quaternion.Euler(hipLeftRotX, hipLeftRotY, hipLeftRotZ) *
Quaternion.identity *
Quaternion.AngleAxis(quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.forward) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.right);
quaternionEuler = body.Joints[JointType.AnkleLeft].Rotation.eulerAngles;;
meshActiveJoints[(int)JointType.KneeLeft].transform.rotation = Quaternion.Euler(kneeLeftRotX, kneeLeftRotY, kneeLeftRotZ) *
Quaternion.identity *
Quaternion.AngleAxis(quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.forward) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.right);
quaternionEuler = body.Joints[JointType.SpineMid].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.SpineBase].transform.rotation = Quaternion.Euler(spineBaseRotX, spineBaseRotY, spineBaseRotZ) *
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.z, Vector3.forward) *
Quaternion.AngleAxis(quaternionEuler.x, Vector3.right);
quaternionEuler = body.Joints[JointType.SpineShoulder].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.SpineMid].transform.rotation = Quaternion.Euler(spineMidRotX, spineMidRotY, spineMidRotZ) *
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.z, Vector3.forward) *
Quaternion.AngleAxis(quaternionEuler.x, Vector3.right);
quaternionEuler = body.Joints[JointType.Neck].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.SpineShoulder].transform.rotation = Quaternion.Euler(spineShoulderRotX, spineShoulderRotY, spineShoulderRotZ) *
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.z, Vector3.forward) *
Quaternion.AngleAxis(quaternionEuler.x, Vector3.right);
quaternionEuler = body.Joints[JointType.Head].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.Head].transform.rotation = Quaternion.Euler(headRotX, headRotY, headRotZ) *
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward);
quaternionEuler = body.Joints[JointType.ElbowRight].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.ShoulderRight].transform.rotation =
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward) *
Quaternion.Euler(shoulderRightRotX, shoulderRightRotY, shoulderRightRotZ);
quaternionEuler = body.Joints[JointType.ElbowLeft].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.ShoulderLeft].transform.rotation =
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward) *
Quaternion.Euler(shoulderLeftRotX, shoulderLeftRotY, shoulderLeftRotZ);
quaternionEuler = body.Joints[JointType.WristRight].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.ElbowRight].transform.rotation =
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward) *
Quaternion.Euler(elbowRightRotX, elbowRightRotY, elbowRightRotZ);
quaternionEuler = body.Joints[JointType.WristLeft].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.ElbowLeft].transform.rotation =
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward) *
Quaternion.Euler(elbowLeftRotX, elbowLeftRotY, elbowLeftRotZ);
quaternionEuler = body.Joints[JointType.HandRight].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.WristRight].transform.rotation =
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward) *
Quaternion.Euler(wristRightRotX, wristRightRotY, wristRightRotZ);
quaternionEuler = body.Joints[JointType.HandLeft].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.WristLeft].transform.rotation =
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward) *
Quaternion.Euler(wristLeftRotX, wristLeftRotY, wristLeftRotZ);
quaternionEuler = body.Joints[JointType.HandRight].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.HandRight].transform.rotation =
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward) *
Quaternion.Euler(handRightRotX, handRightRotY, handRightRotZ);
quaternionEuler = body.Joints[JointType.HandLeft].Rotation.eulerAngles;
meshActiveJoints[(int)JointType.HandLeft].transform.rotation =
Quaternion.identity *
Quaternion.AngleAxis(-quaternionEuler.y, Vector3.up) *
Quaternion.AngleAxis(-quaternionEuler.x, Vector3.right) *
Quaternion.AngleAxis(quaternionEuler.z, Vector3.forward) *
Quaternion.Euler(handLeftRotX, handLeftRotY, handLeftRotZ);
}
private Avatar.Body GetMovingExpAverageData(Avatar.Body lastBody)
{
if (avgJoints != null)
{
for (JointType jt = 0; jt <= JointType.ThumbRight; jt++)
{
avgJoints[jt].Position = ((lastBody.Joints[jt].Position - avgJoints[jt].Position) * k) + avgJoints[jt].Position;
avgJoints[jt].Rotation = Quaternion.identity;
}
for (int i = BodySamples.Count - 1; i >= 0; i--)
{
for (JointType jt = 0; jt <= JointType.ThumbRight; jt++)
{
Quaternion avgRot = avgJoints[jt].Rotation;
Quaternion sampleRot = BodySamples[i].Joints[jt].Rotation;
avgJoints[jt].Rotation = new Quaternion(
avgRot.x + sampleRot.x,
avgRot.y + sampleRot.y,
avgRot.z + sampleRot.z,
avgRot.w + sampleRot.w);
if (i == 0)
{
avgJoints[jt].TrackingState = BodySamples[i].Joints[jt].TrackingState;
}
}
}
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
Quaternion avgRot = avgJoints[jt].Rotation;
float k = 1.0f / Mathf.Sqrt(avgRot.x * avgRot.x + avgRot.y * avgRot.y + avgRot.z * avgRot.z + avgRot.w * avgRot.w);
avgJoints[jt].Rotation = new Quaternion(avgRot.x * k, avgRot.y * k, avgRot.z * k, avgRot.w * k);
}
return(new Avatar.Body(avgJoints));
}
else
{
avgJoints = new Dictionary <JointType, Avatar.Joint>();
for (int i = BodySamples.Count - 1; i >= 0; i--)
{
for (JointType jt = 0; jt <= JointType.ThumbRight; jt++)
{
if (avgJoints.ContainsKey(jt))
{
avgJoints[jt].Position += BodySamples[i].Joints[jt].Position;
Quaternion avgRot = avgJoints[jt].Rotation;
Quaternion sampleRot = BodySamples[i].Joints[jt].Rotation;
avgJoints[jt].Rotation = new Quaternion(
avgRot.x + sampleRot.x,
avgRot.y + sampleRot.y,
avgRot.z + sampleRot.z,
avgRot.w + sampleRot.w);
}
else
{
avgJoints[jt] = new Avatar.Joint();
avgJoints[jt].Position = BodySamples[i].Joints[jt].Position;
avgJoints[jt].Rotation = BodySamples[i].Joints[jt].Rotation;
avgJoints[jt].JointType = BodySamples[i].Joints[jt].JointType;
}
//tracking state
if (i == 0)
{
avgJoints[jt].TrackingState = BodySamples[i].Joints[jt].TrackingState;
}
}
}
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
avgJoints[jt].Position = avgJoints[jt].Position / (float)MaxSamples;
Quaternion avgRot = avgJoints[jt].Rotation;
float k = 1.0f / Mathf.Sqrt(avgRot.x * avgRot.x + avgRot.y * avgRot.y + avgRot.z * avgRot.z + avgRot.w * avgRot.w);
avgJoints[jt].Rotation = new Quaternion(avgRot.x * k, avgRot.y * k, avgRot.z * k, avgRot.w * k);
}
}
return(new Avatar.Body(avgJoints));
}
