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