public static CompositeBodyTransform CrossoverArmStretchTransform(JointSide sideOfStretchedArm)
{
JointSide oppositeSide;
JointTransform stretchDirectionTransform;
// First set the stretch direction and the opposite side (helper arm side)
if (sideOfStretchedArm == JointSide.Left)
{
oppositeSide = JointSide.Right;
stretchDirectionTransform = new SetJointDirectionTransform(1, 0, 0);
}
else
{
oppositeSide = JointSide.Left;
stretchDirectionTransform = new SetJointDirectionTransform(-1, 0, 0);
}
// Get all joint types for both stretched and support arm
JointType stretchedElbow = JointTypeHelper.GetSidedJointType(SidedJointName.Elbow, sideOfStretchedArm);
JointType stretchedWrist = JointTypeHelper.GetSidedJointType(SidedJointName.Wrist, sideOfStretchedArm);
JointType supportElbow = JointTypeHelper.GetSidedJointType(SidedJointName.Elbow, oppositeSide);
JointType supportWrist = JointTypeHelper.GetSidedJointType(SidedJointName.Wrist, oppositeSide);
// Apply the transforms in both arms
return(new CompositeBodyTransform()
.Compose(stretchedElbow, stretchDirectionTransform)
.Compose(stretchedWrist, stretchDirectionTransform)
.Compose(supportElbow, new SetJointDirectionTransform(0, -1, 0))
.Compose(supportWrist, new SetJointDirectionTransform(0, 1, 0)));
}
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false)
: base(body =>
{
JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
Z3Point3D joint1Position = body.GetJointPosition(jointType);
Z3Point3D joint2Position = body.GetJointPosition(sidedHandType);
BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold);
if (dont)
{
expr = Z3.Context.MkNot(expr);
}
return(expr);
},
jointType,
JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide))
{
this.JointType = jointType;
this.HandSide = handSide;
if (dont)
{
isNegated = true;
}
else
{
isNegated = false;
}
}
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false)
: base(body =>
{
JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
Z3Point3D joint1Position = body.GetJointZ3Position(jointType);
Z3Point3D joint2Position = body.GetJointZ3Position(sidedHandType);
BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold);
if (dont)
{
expr = Z3.Context.MkNot(expr);
}
return(expr);
},
body =>
{
var stopwatch = new Stopwatch();
stopwatch.Start();
var sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
var p1 = body.Positions[jointType];
var p2 = body.Positions[sidedHandType];
var distance = Math.Max(0.00000001, p1.DistanceTo(p2));
var percentage = Math.Min(1.0, distanceThreshold / distance);
if (dont)
{
percentage = Math.Min(1.0, distance / distanceThreshold);
}
//Console.WriteLine("tch " + stopwatch.ElapsedTicks);
return(percentage);
},
jointType,
JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide))
{
this.JointType = jointType;
this.HandSide = handSide;
if (dont)
{
isNegated = true;
}
else
{
isNegated = false;
}
}
