public Voxel ModifyEdge(bool isVoxelSet, int edge, float intersection, float3 normal)
{
float4x3 newData = (float4x3)Data;
newData[edge] = new float4(normal, intersection);
return(new Voxel(isVoxelSet, Material, new float3(newData[0].w, newData[1].w, newData[2].w), new float3x3(newData[0].xyz, newData[1].xyz, newData[2].xyz)));
}
public void Execute(int startIndex, int count)
{
for (int i = startIndex; i < startIndex + count; ++i)
{
//Calculate Cost of Pose Joint Positions
float4x3 jointPosDiff = InputPoses[i].JointPositions - DesiredPose.JointPositions;
float4 jointPosDist = ((jointPosDiff.c0 * jointPosDiff.c0) +
(jointPosDiff.c1 * jointPosDiff.c1) +
(jointPosDiff.c2 * jointPosDiff.c2)) * JointPositionWeights;
//Calculate Cost of Pose Velocity & Joints Velocity
float4x3 jointVelDiff = InputPoses[i].JointVelocities - DesiredPose.JointVelocities;
float4 jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeights;
float3 bodyVel = (InputPoses[i].BodyVelocity - DesiredPose.BodyVelocity);
float bodyVelDist = ((bodyVel.x * bodyVel.x) +
(bodyVel.y * bodyVel.y) +
(bodyVel.z * bodyVel.z)) * BodyVelocityWeight;
GoalCosts[i] = jointVelDist.x + jointVelDist.y + jointVelDist.z + jointVelDist.w
+ jointPosDist.x + jointPosDist.y + jointPosDist.z + bodyVelDist;
}
}
/// <summary>
/// Builds a 4x4 matrix transform from an affine transformation.
/// </summary>
public static float4x4 FromAffine(float4x3 affine)
{
return(float4x4(
affine._m00, affine._m01, affine._m02, 0,
affine._m10, affine._m11, affine._m12, 0,
affine._m20, affine._m21, affine._m22, 0,
affine._m30, affine._m31, affine._m32, 1
));
}
public void Execute(int i)
{
float4x3 p = TransformPositions(domainTRS, transpose(positions[i]));
int4 u = (int4)floor(p.c0);
int4 v = (int4)floor(p.c1);
int4 w = (int4)floor(p.c2);
hashes[i] = hash.Eat(u).Eat(v).Eat(w);
}
private static void CustomVisit(ref float4x3 f)
{
LogVisit(f);
GUILayout.Label(name);
EditorGUI.indentLevel++;
f[0] = EditorGUILayout.Vector4Field("", (Vector4)f[0]);
f[1] = EditorGUILayout.Vector4Field("", (Vector4)f[1]);
f[2] = EditorGUILayout.Vector4Field("", (Vector4)f[2]);
EditorGUI.indentLevel--;
}
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].JointPositions - DesiredPose.JointPositions;
float3 jointPosDist = ((jointPosDiff.c0 * jointPosDiff.c0) +
(jointPosDiff.c1 * jointPosDiff.c1) +
(jointPosDiff.c2 * jointPosDiff.c2)) * JointPositionWeights;
float4x3 jointVelocities = InputPoses[i].JointVelocities;
//Calculate Cost of Pose Velocity & Joints Velocity
float4x3 jointVelDiff = jointVelocities - DesiredPose.JointVelocities;
float4 jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeights;
//Calculate resultsant pose
float4x3 jointResultVel = new float4x3(new float4(jointPosDiff.c0, 0f),
new float4(jointPosDiff.c1, 0f),
new float4(jointPosDiff.c2, 0f)) / PoseInterval;
float4x3 jointResultVelDiff = jointVelocities - jointResultVel;
float4 jointResultVelDist = ((jointResultVelDiff.c0 * jointResultVelDiff.c0) +
(jointResultVelDiff.c1 * jointResultVelDiff.c1) +
(jointResultVelDiff.c2 * jointResultVelDiff.c2)) * (JointVelocityWeights * ResultantVelocityWeight);
//Calculate final total cost of the pose
GoalCosts[i] = jointVelDist.x + jointVelDist.y + jointVelDist.z + jointVelDist.w
+ jointPosDist.x + jointPosDist.y + jointPosDist.z +
jointResultVelDist.x + jointResultVelDist.y + jointResultVelDist.z;
}
}
public void Execute(int startIndex, int count)
{
for (int i = startIndex; i < startIndex + count; ++i)
{
//Calculate Cost of Pose Joint Positions
float4x3 jointPosDiff = InputPoses[i].JointPositionsA - DesiredPose.JointPositionsA;
float4 jointPosDist = ((jointPosDiff.c0 * jointPosDiff.c0) +
(jointPosDiff.c1 * jointPosDiff.c1) +
(jointPosDiff.c2 * jointPosDiff.c2)) * JointPositionWeightsA;
//Calculate Cost of Pose Velocity & Joints Velocity
float4x3 jointVelDiff = InputPoses[i].JointVelocitiesA - DesiredPose.JointVelocitiesA;
float4 jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeightsA;
GoalCosts[i] = jointVelDist.x + jointVelDist.y + jointVelDist.z + jointVelDist.w
+ jointPosDist.x + jointPosDist.y + jointPosDist.z + jointPosDist.w;
float3x3 jointPosDiffB = InputPoses[i].JointPositionsB - DesiredPose.JointPositionsB;
float3 jointPosDistB = ((jointPosDiffB.c0 * jointPosDiffB.c0) +
(jointPosDiffB.c1 * jointPosDiffB.c1) +
(jointPosDiffB.c2 * jointPosDiffB.c2)) * JointPositionWeightsB;
jointVelDiff = InputPoses[i].JointVelocitiesB - DesiredPose.JointVelocitiesB;
jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeightsB;
GoalCosts[i] += jointVelDist.x + jointVelDist.y + jointVelDist.z + jointVelDist.w
+ jointPosDistB.x + jointPosDistB.y + jointPosDistB.z;
}
}
public static void AreEqual(float4x3 a, float4x3 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(float4x3 a, float4x3 b, float delta = 0.0f)
{
AreEqual(a.c0, b.c0, delta);
AreEqual(a.c1, b.c1, delta);
AreEqual(a.c2, b.c2, delta);
}
public static void AreEqual(float4x3 expected, float4x3 actual, int maxUlp, bool signedZeroEqual)
{
AreEqual(expected.c0, actual.c0, maxUlp, signedZeroEqual);
AreEqual(expected.c1, actual.c1, maxUlp, signedZeroEqual);
AreEqual(expected.c2, actual.c2, maxUlp, signedZeroEqual);
}
public static void AreEqual(float4x3 expected, float4x3 actual, float delta = 0.0f)
{
AreEqual(expected.c0, actual.c0, delta);
AreEqual(expected.c1, actual.c1, delta);
AreEqual(expected.c2, actual.c2, delta);
}
float4x3 TransformPositions(float3x4 trs, float4x3 p) => float4x3(
trs.c0.x * p.c0 + trs.c1.x * p.c1 + trs.c2.x * p.c2 + trs.c3.x,
trs.c0.y * p.c0 + trs.c1.y * p.c1 + trs.c2.y * p.c2 + trs.c3.y,
trs.c0.z * p.c0 + trs.c1.z * p.c1 + trs.c2.z * p.c2 + trs.c3.z
);
float4x3 TransformVectors(float3x4 trs, float4x3 p, float w = 1f) => float4x3(
trs.c0.x * p.c0 + trs.c1.x * p.c1 + trs.c2.x * p.c2 + trs.c3.x * w,
trs.c0.y * p.c0 + trs.c1.y * p.c1 + trs.c2.y * p.c2 + trs.c3.y * w,
trs.c0.z * p.c0 + trs.c1.z * p.c1 + trs.c2.z * p.c2 + trs.c3.z * w
);
public void Execute(int startIndex, int count)
{
for (int i = startIndex; i < startIndex + count; ++i)
{
//Calculate Cost of Pose Joint Positions
float4x3 jointPosDiff = InputPoses[i].JointPositionsA - DesiredPose.JointPositionsA;
float4 jointPosDist = ((jointPosDiff.c0 * jointPosDiff.c0) +
(jointPosDiff.c1 * jointPosDiff.c1) +
(jointPosDiff.c2 * jointPosDiff.c2)) * JointPositionWeightsA;
float4x3 jointVelocities = InputPoses[i].JointVelocitiesA;
//Calculate Cost of Pose Velocity & Joints Velocity
float4x3 jointVelDiff = jointVelocities - DesiredPose.JointVelocitiesA;
float4 jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeightsA;
float3 bodyVel = (InputPoses[i].BodyVelocity - DesiredPose.BodyVelocity);
float bodyVelDist = ((bodyVel.x * bodyVel.x) +
(bodyVel.y * bodyVel.y) +
(bodyVel.z * bodyVel.z)) * BodyVelocityWeight;
//Calculate resultant poses
float4x3 jointResultVel = jointPosDiff / PoseInterval;
float4x3 jointResultVelDiff = jointVelocities - jointResultVel;
float4 jointResultVelDist = ((jointResultVelDiff.c0 * jointResultVelDiff.c0) +
(jointResultVelDiff.c1 * jointResultVelDiff.c1) +
(jointResultVelDiff.c2 * jointResultVelDiff.c2)) * (JointVelocityWeightsA * ResultantVelocityWeight);
float4 localCost = jointVelDist + jointPosDist + jointResultVelDist;
//Calculate Cost of Pose Joint Positions
jointPosDiff = InputPoses[i].JointPositionsB - DesiredPose.JointPositionsB;
jointPosDist = ((jointPosDiff.c0 * jointPosDiff.c0) +
(jointPosDiff.c1 * jointPosDiff.c1) +
(jointPosDiff.c2 * jointPosDiff.c2)) * JointPositionWeightsB;
jointVelocities = InputPoses[i].JointPositionsB;
//Calculate Cost of Pose Velocity & Joints Velocity
jointVelDiff = jointVelocities - DesiredPose.JointVelocitiesB;
jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeightsB;
//Calculate resultant poses
jointResultVel = jointPosDiff / PoseInterval;
jointResultVelDiff = jointVelocities - jointResultVel;
jointResultVelDist = ((jointResultVelDiff.c0 * jointResultVelDiff.c0) +
(jointResultVelDiff.c1 * jointResultVelDiff.c1) +
(jointResultVelDiff.c2 * jointResultVelDiff.c2)) * (JointVelocityWeightsA * ResultantVelocityWeight);
localCost += jointVelDist + jointPosDist + jointResultVelDist;
GoalCosts[i] = localCost.x + localCost.y + localCost.z + localCost.w + bodyVelDist;
}
}
public void Execute(int startIndex, int count)
{
for (int i = startIndex; i < startIndex + count; ++i)
{
//Calculate Cost of Pose Joint Positions
float4x3 jointPosDiff = InputPoses[i].JointPositionsA - DesiredPose.JointPositionsA;
float4 jointPosDist = ((jointPosDiff.c0 * jointPosDiff.c0) +
(jointPosDiff.c1 * jointPosDiff.c1) +
(jointPosDiff.c2 * jointPosDiff.c2)) * JointPositionWeightsA;
float4x3 jointVelocities = InputPoses[i].JointVelocitiesA;
//Calculate Cost of Pose Velocity & Joints Velocity
float4x3 jointVelDiff = jointVelocities - DesiredPose.JointVelocitiesA;
float4 jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeightsA;
//Calculate resultant pose
float4x3 jointResultVelA = jointPosDiff / PoseInterval;
float4x3 jointResultVelDiffA = jointVelocities - jointResultVelA;
float4 jointResultVelDistA = ((jointResultVelDiffA.c0 * jointResultVelDiffA.c0) +
(jointResultVelDiffA.c1 * jointResultVelDiffA.c1) +
(jointResultVelDiffA.c2 * jointResultVelDiffA.c2)) * (JointVelocityWeightsA * ResultantVelocityWeight);
GoalCosts[i] = jointVelDist.x + jointVelDist.y + jointVelDist.z + jointVelDist.w
+ jointPosDist.x + jointPosDist.y + jointPosDist.z + jointPosDist.w
+ jointResultVelDistA.x + jointResultVelDistA.y + jointResultVelDistA.z + jointResultVelDistA.w;
float3x3 jointPosDiffB = InputPoses[i].JointPositionsB - DesiredPose.JointPositionsB;
float3 jointPosDistB = ((jointPosDiffB.c0 * jointPosDiffB.c0) +
(jointPosDiffB.c1 * jointPosDiffB.c1) +
(jointPosDiffB.c2 * jointPosDiffB.c2)) * JointPositionWeightsB;
jointVelocities = InputPoses[i].JointVelocitiesB;
jointVelDiff = jointVelocities - DesiredPose.JointVelocitiesB;
jointVelDist = ((jointVelDiff.c0 * jointVelDiff.c0) +
(jointVelDiff.c1 * jointVelDiff.c1) +
(jointVelDiff.c2 * jointVelDiff.c2)) * JointVelocityWeightsB;
//Calculate resultant poses
float4x3 jointResultVel = new float4x3(new float4(jointPosDiffB.c0, 0f),
new float4(jointPosDiffB.c1, 0f),
new float4(jointPosDiffB.c2, 0f)) / PoseInterval;
float4x3 jointResultVelDiff = jointVelocities - jointResultVel;
float4 jointResultVelDist = ((jointResultVelDiff.c0 * jointResultVelDiff.c0) +
(jointResultVelDiff.c1 * jointResultVelDiff.c1) +
(jointResultVelDiff.c2 * jointResultVelDiff.c2)) * (JointVelocityWeightsB * ResultantVelocityWeight);
GoalCosts[i] += jointVelDist.x + jointVelDist.y + jointVelDist.z + jointVelDist.w
+ jointPosDistB.x + jointPosDistB.y + jointPosDistB.z
+ jointResultVelDist.x + jointResultVelDist.y + jointResultVelDist.z;
}
}
