public static IBodyRestriction DontTwistHipRestriction(int degreesThreshold = 15)
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
var result = new SimpleBodyRestriction(body =>
{
// Shoulders, hips and feet must be aligned
Z3Point3D leftToRightShoulderVec = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight];
Z3Point3D leftToRightHipVec = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight];
Z3Point3D leftAnklePosition =
body.Joints[JointType.HipLeft] +
body.Joints[JointType.KneeLeft] +
body.Joints[JointType.AnkleLeft];
Z3Point3D rightAnklePosition =
body.Joints[JointType.HipRight] +
body.Joints[JointType.KneeRight] +
body.Joints[JointType.AnkleRight];
Z3Point3D leftToRightAnkleVec = rightAnklePosition - leftAnklePosition;
BoolExpr expr1 = leftToRightShoulderVec.IsDegreesBetweenLessThan(leftToRightHipVec, degreesThreshold);
BoolExpr expr2 = leftToRightShoulderVec.IsDegreesBetweenLessThan(leftToRightAnkleVec, degreesThreshold);
BoolExpr expr3 = leftToRightAnkleVec.IsDegreesBetweenLessThan(leftToRightHipVec, degreesThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return(expr);
},
body => { return(1.0); });
return(result);
}
public static IBodyRestriction DistributeWeightRestriction(int degreesThreshold = 15)
{
var result = new SimpleBodyRestriction(body =>
{
Z3Point3D up = new Z3Point3D(0, 1, 0);
Z3Point3D legLeft = body.Joints[JointType.KneeLeft] + body.Joints[JointType.AnkleLeft];
Z3Point3D legRight = body.Joints[JointType.KneeRight] + body.Joints[JointType.AnkleRight];
Z3Point3D legsAverage = (legLeft + legRight) / 2;
BoolExpr expr = legsAverage.IsDegreesBetweenLessThan(up, degreesThreshold);
return(expr);
},
body => { return(1.0); });
return(result);
}