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; } }