public static void AreEqual(float2x3 a, float2x3 b, int maxUlp, bool signedZeroEqual)
{
AreEqual(a.c0, b.c0, maxUlp, signedZeroEqual);
AreEqual(a.c1, b.c1, maxUlp, signedZeroEqual);
AreEqual(a.c2, b.c2, maxUlp, signedZeroEqual);
}
public static void AreEqual(float2x3 a, float2x3 b, float delta = 0.0f)
{
AreEqual(a.c0, b.c0, delta);
AreEqual(a.c1, b.c1, delta);
AreEqual(a.c2, b.c2, delta);
}
public void Execute(int startIndex, int count)
{
for (int i = startIndex; i < startIndex + count; ++i)
{
//Calculate Cost of Pose Joint Positions
float3x3 jointPosDiff = InputPoses[i].JointPositionsA - DesiredPose.JointPositionsA;
float3 jointPosDist = ((jointPosDiff.c0 * jointPosDiff.c0) +
(jointPosDiff.c1 * jointPosDiff.c1) +
(jointPosDiff.c2 * jointPosDiff.c2)) * JointPositionWeightsA;
float3x3 jointVelocitiesA = InputPoses[i].JointVelocitiesA;
//Calculate Cost of Pose Velocity & Joints Velocity
float3x3 jointVelDiff = jointVelocitiesA - DesiredPose.JointVelocitiesA;
float3 jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeightsA;
//Calculate resultsant pose A
float3x3 jointResultVelA = jointPosDiff / PoseInterval;
float3x3 jointResultVelDiffA = jointVelocitiesA - jointResultVelA;
float3 jointResultVelDistA = ((jointResultVelDiffA.c0 * jointResultVelDiffA.c0) +
(jointResultVelDiffA.c1 * jointResultVelDiffA.c1) +
(jointResultVelDiffA.c2 * jointResultVelDiffA.c2)) * (JointVelocityWeightsB * ResultantVelocityWeight);
GoalCosts[i] = jointVelDist.x + jointVelDist.y + jointVelDist.z
+ jointPosDist.x + jointPosDist.y + jointPosDist.z
+ jointResultVelDistA.x + jointResultVelDistA.y + jointResultVelDistA.z;
float2x3 jointPosDiffB = InputPoses[i].JointPositionsB - DesiredPose.JointPositionsB;
float2 jointPosDistB = ((jointPosDiffB.c0 * jointPosDiffB.c0) +
(jointPosDiffB.c1 * jointPosDiffB.c1) +
(jointPosDiffB.c2 * jointPosDiffB.c2)) * JointPositionWeightsB;
float3x3 jointVelocitiesB = InputPoses[i].JointVelocitiesB;
jointVelDiff = jointVelocitiesB - DesiredPose.JointVelocitiesB;
jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeightsB;
//Calculate resultant pose B
float3x3 jointResultVelB = new float3x3(new float3(jointPosDiffB.c0, 0f),
new float3(jointPosDiffB.c1, 0f),
new float3(jointPosDiffB.c2, 0f));
float3x3 jointResultVelDiffB = jointVelocitiesB - jointResultVelB;
float3 jointResultVelDistB = ((jointResultVelDiffB.c0 * jointResultVelDiffB.c0) +
(jointResultVelDiffB.c1 * jointResultVelDiffB.c1) +
(jointResultVelDiffB.c2 * jointResultVelDiffB.c2)) * (JointVelocityWeightsB * ResultantVelocityWeight);
GoalCosts[i] += jointVelDist.x + jointVelDist.y + jointVelDist.z
+ jointPosDistB.x + jointPosDistB.y
+ jointResultVelDistB.x + jointResultVelDistB.y;
}
}
