private void animateLower(float[] angles)
{
float[] waistAngles = new float[] { angles[1], angles[2], angles[3] };
chainLower = jointRotations.rotateWaist(waistAngles, new float[] { 0, 0, 0 }, hmd);
waist.localRotation = chainLower;
float[] ltAngles = new float[] { angles[5], angles[6], angles[7] };
chainLower = jointRotations.rotateLeftLeg(ltAngles, waist.localRotation,
hmd);
leftThigh.localRotation = chainLower;
float[] lsAngles = new float[] { angles[9], angles[10], angles[11] };
chainLower = jointRotations.rotateLeftShin(lsAngles, waist.localRotation,
leftThigh.localRotation, hmd);
leftShin.localRotation = chainLower;
float[] rtAngles = new float[] { angles[13], angles[14], angles[15] };
chainLower = jointRotations.rotateRightLeg(rtAngles, waist.localRotation,
hmd);
rightThigh.localRotation = chainLower;
float[] rsAngles = new float[] { angles[17], angles[18], angles[19] };
chainLower = jointRotations.rotateRightShin(rsAngles, waist.localRotation,
rightThigh.localRotation, hmd);
rightShin.localRotation = chainLower;
}
private void OnLowerBodyAnglesChanged(HumanoidAngles <Vector3> absoluteAngles)
{
var headsetRotation = Quaternion.identity;
var initialWaistYaw = WaistBaseOrientation.z;
var initialWaistAngles = Quaternion.AngleAxis(initialWaistYaw, Vector3.up);
_yawAdjustedWaistAngles[0] = absoluteAngles.Waist.x;
_yawAdjustedWaistAngles[1] = absoluteAngles.Waist.y;
_yawAdjustedWaistAngles[2] = absoluteAngles.Waist.z;
_yawAdjustedLeftUpperLegAngles[0] = absoluteAngles.LeftUpperLeg.x;
_yawAdjustedLeftUpperLegAngles[1] = absoluteAngles.LeftUpperLeg.y;
_yawAdjustedLeftUpperLegAngles[2] = absoluteAngles.LeftUpperLeg.z;
_yawAdjustedLeftLowerLegAngles[0] = absoluteAngles.LeftLowerLeg.x;
_yawAdjustedLeftLowerLegAngles[1] = absoluteAngles.LeftLowerLeg.y;
_yawAdjustedLeftLowerLegAngles[2] = absoluteAngles.LeftLowerLeg.z;
_yawAdjustedRightUpperLegAngles[0] = absoluteAngles.RightUpperLeg.x;
_yawAdjustedRightUpperLegAngles[1] = absoluteAngles.RightUpperLeg.y;
_yawAdjustedRightUpperLegAngles[2] = absoluteAngles.RightUpperLeg.z;
_yawAdjustedRightLowerLegAngles[0] = absoluteAngles.RightLowerLeg.x;
_yawAdjustedRightLowerLegAngles[1] = absoluteAngles.RightLowerLeg.y;
_yawAdjustedRightLowerLegAngles[2] = absoluteAngles.RightLowerLeg.z;
// Transform absolute lower body angles into relative ones
var localAngleWaist = _jointRotations.rotateWaist(
_yawAdjustedWaistAngles,
initialWaistAngles,
headsetRotation
);
var localAngleLeftUpperLeg = _jointRotations.rotateLeftLeg(
_yawAdjustedLeftUpperLegAngles,
localAngleWaist,
initialWaistAngles
);
var localAngleLeftLowerLeg = _jointRotations.rotateLeftShin(
_yawAdjustedLeftLowerLegAngles,
localAngleWaist,
localAngleLeftUpperLeg,
initialWaistAngles
);
var localAngleRightUpperLeg = _jointRotations.rotateRightLeg(
_yawAdjustedRightUpperLegAngles,
localAngleWaist,
initialWaistAngles
);
var localAngleRightLowerLeg = _jointRotations.rotateRightShin(
_yawAdjustedRightLowerLegAngles,
localAngleWaist,
localAngleRightUpperLeg,
initialWaistAngles
);
LocalAngles.SetLowerBodyAngles(localAngleWaist, localAngleLeftUpperLeg, localAngleLeftLowerLeg,
localAngleRightUpperLeg, localAngleRightLowerLeg);
}
public void operate(float[] angles)
{
if (angles != null)
{
if (initState == InitState.PREINIT && angles != null)
{
if (!useCore)
{
Buffer.BlockCopy((float[])angles.Clone(), 1 * sizeof(float), initWaist, 0, 3 * sizeof(float));
}
else
{
initWaist = upperReference.getCoreInit();
}
setInitRot();
}
else if (initState == InitState.INIT)
{
if (!useCore)
{
//core node 1
float[] waistAngles = new float[] { angles[1], angles[2], angles[3] - initWaist[2] };
chain = jointRotations.rotateWaist(waistAngles, initWaist, refCoord.localRotation);
}
else
{
chain = core.localRotation;
}
waistPose.Enqueue(chain);
//Left Thigh
float[] ltAngles = new float[] { angles[5], angles[6], angles[7] - initWaist[2] };
chain = jointRotations.rotateLeftLeg(ltAngles, waist.localRotation,
refCoord.localRotation);
leftThighPose.Enqueue(chain);
//Left shin
float[] lsAngles = new float[] { angles[9], angles[10], angles[11] - initWaist[2] };
chain = jointRotations.rotateLeftShin(lsAngles, waist.localRotation,
leftThigh.localRotation, refCoord.localRotation);
leftShinPose.Enqueue(chain);
//Right Thigh
float[] rtAngles = new float[] { angles[13], angles[14], angles[15] - initWaist[2] };
chain = jointRotations.rotateRightLeg(rtAngles, waist.localRotation,
refCoord.localRotation);
rightThighPose.Enqueue(chain);
//Right shin
float[] rsAngles = new float[] { angles[17], angles[18], angles[19] - initWaist[2] };
chain = jointRotations.rotateRightShin(rsAngles, waist.localRotation,
rightThigh.localRotation, refCoord.localRotation);
rightShinPose.Enqueue(chain);
}
}
}
