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;
}
}
public static JointType GetSidedJointType(SidedJointName name, JointSide side)
{
switch (name)
{
case (SidedJointName.Ankle):
return((side == JointSide.Left) ? JointType.AnkleLeft : JointType.AnkleRight);
case (SidedJointName.Elbow):
return((side == JointSide.Left) ? JointType.ElbowLeft : JointType.ElbowRight);
case (SidedJointName.Foot):
return((side == JointSide.Left) ? JointType.FootLeft : JointType.FootRight);
case (SidedJointName.Hand):
return((side == JointSide.Left) ? JointType.HandLeft : JointType.HandRight);
case (SidedJointName.HandTip):
return((side == JointSide.Left) ? JointType.HandTipLeft : JointType.HandTipRight);
case (SidedJointName.Hip):
return((side == JointSide.Left) ? JointType.HipLeft : JointType.HipRight);
case (SidedJointName.Knee):
return((side == JointSide.Left) ? JointType.KneeLeft : JointType.KneeRight);
case (SidedJointName.Shoulder):
return((side == JointSide.Left) ? JointType.ShoulderLeft : JointType.ShoulderRight);
case (SidedJointName.Thumb):
return((side == JointSide.Left) ? JointType.ThumbLeft : JointType.ThumbRight);
case (SidedJointName.Wrist):
return((side == JointSide.Left) ? JointType.WristLeft : JointType.WristRight);
default:
throw new ArgumentException();
}
}
// Arm considers only the elbow and the wrist,
// This decision is taken based on common sense that if you ask
// the user to put his arm in a specific direction,
// he may do it with his hand pointing another direction and
// it would still be correct.
public static IEnumerable<JointType> GetArm(JointSide side)
{
yield return GetSidedJointType(SidedJointName.Wrist, side);
yield return GetSidedJointType(SidedJointName.Elbow, side);
}
public static JointType GetSidedJointType(SidedJointName name, JointSide side)
{
switch (name)
{
case (SidedJointName.Ankle):
return (side == JointSide.Left) ? JointType.AnkleLeft : JointType.AnkleRight;
case (SidedJointName.Elbow):
return (side == JointSide.Left) ? JointType.ElbowLeft : JointType.ElbowRight;
case (SidedJointName.Foot):
return (side == JointSide.Left) ? JointType.FootLeft : JointType.FootRight;
case (SidedJointName.Hand):
return (side == JointSide.Left) ? JointType.HandLeft : JointType.HandRight;
case (SidedJointName.HandTip):
return (side == JointSide.Left) ? JointType.HandTipLeft : JointType.HandTipRight;
case (SidedJointName.Hip):
return (side == JointSide.Left) ? JointType.HipLeft : JointType.HipRight;
case (SidedJointName.Knee):
return (side == JointSide.Left) ? JointType.KneeLeft : JointType.KneeRight;
case (SidedJointName.Shoulder):
return (side == JointSide.Left) ? JointType.ShoulderLeft : JointType.ShoulderRight;
case (SidedJointName.Thumb):
return (side == JointSide.Left) ? JointType.ThumbLeft : JointType.ThumbRight;
case (SidedJointName.Wrist):
return (side == JointSide.Left) ? JointType.WristLeft : JointType.WristRight;
default:
throw new ArgumentException();
}
}
public static IEnumerable<JointType> GetLeg(JointSide side)
{
yield return GetSidedJointType(SidedJointName.Knee, side);
yield return GetSidedJointType(SidedJointName.Ankle, side);
}
internal Wrapper(JointSide side)
{
this.value = side;
}
public static IEnumerable <JointType> GetLeg(JointSide side)
{
yield return(GetSidedJointType(SidedJointName.Knee, side));
yield return(GetSidedJointType(SidedJointName.Ankle, side));
}
// Arm considers only the elbow and the wrist,
// This decision is taken based on common sense that if you ask
// the user to put his arm in a specific direction,
// he may do it with his hand pointing another direction and
// it would still be correct.
public static IEnumerable <JointType> GetArm(JointSide side)
{
yield return(GetSidedJointType(SidedJointName.Wrist, side));
yield return(GetSidedJointType(SidedJointName.Elbow, side));
}
public static BodyTransform 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 BodyTransform()
.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.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;
}
}
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;
}
}
