public static IBodyRestriction DontTwistHipRestriction(int angleThreshold = 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.IsAngleBetweenLessThan(leftToRightHipVec, angleThreshold); BoolExpr expr2 = leftToRightShoulderVec.IsAngleBetweenLessThan(leftToRightAnkleVec, angleThreshold); BoolExpr expr3 = leftToRightAnkleVec.IsAngleBetweenLessThan(leftToRightHipVec, angleThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3); return(expr); }); return(result); }
public static IBodyRestriction DistributeWeightRestriction(int angleThreshold = 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.IsAngleBetweenLessThan(up, angleThreshold); return(expr); }); return(result); }
/// <summary> /// This is a basic safety check to make sure we don't break any bones. /// </summary> /// <returns>True if safe</returns> public static IBodyRestriction DefaultSafetyRestriction() { var result = new CompositeBodyRestriction(); int inclinationThreshold = 45; // Head // Make sure neck is not inclinated beyond the threshold var head = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); BoolExpr expr1 = body.Joints[JointType.Head].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr2 = body.Joints[JointType.Neck].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return(expr); }); result.And(head); // Spine // Make sure spine is not inclinated beyond the threshold var spine = new SimpleBodyRestriction(body => { Z3Point3D up = new Z3Point3D(0, 1, 0); BoolExpr expr1 = body.Joints[JointType.SpineMid].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsAngleBetweenLessThan(up, inclinationThreshold); BoolExpr expr3 = body.Joints[JointType.SpineMid].IsAngleBetweenLessThan( body.Joints[JointType.SpineShoulder], inclinationThreshold); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3); return(expr); }); result.And(spine); // Shoulders // Make sure shoulders are not bent var shoulders = new SimpleBodyRestriction(body => { BoolExpr expr = body.Joints[JointType.SpineMid].IsAngleBetweenGreaterThan( body.Joints[JointType.SpineShoulder], 120); return(expr); }); result.And(shoulders); // Elbows // Make sure elbows are not behind the back // And also that they are not on the top/back sub-space var elbows = new SimpleBodyRestriction(body => { BoolExpr exprRight1 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero), Z3.Context.MkLt(body.Joints[JointType.ElbowRight].X, Z3Math.Zero))); BoolExpr exprRight2 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowRight].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowRight].Y, Z3Math.Zero))); BoolExpr exprLeft1 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].X, Z3Math.Zero))); BoolExpr exprLeft2 = Z3.Context.MkNot( Z3.Context.MkAnd( Z3.Context.MkLt(body.Joints[JointType.ElbowLeft].Z, Z3Math.Zero), Z3.Context.MkGt(body.Joints[JointType.ElbowLeft].Y, Z3Math.Zero))); BoolExpr expr = Z3.Context.MkAnd(exprLeft1, exprLeft2, exprRight1, exprRight2); return(expr); }); result.And(elbows); // Wrists // Make sure the inclination of wrists towards the back is not higher than the inclinatin of the elbows // unless elbows are up or wrists are directed to torso // TODO // Hips // Make sure hips are aligned with the shoulders or at lest within the range var hips = new SimpleBodyRestriction(body => { Z3Point3D shouldersSum = body.Joints[JointType.ShoulderLeft].GetInverted() + body.Joints[JointType.ShoulderRight]; Z3Point3D hipsSum = body.Joints[JointType.HipLeft].GetInverted() + body.Joints[JointType.HipRight]; BoolExpr expr = shouldersSum.IsAngleBetweenLessThan(hipsSum, 45); return(expr); }); result.And(hips); // Legs // Make sure legs are not higher than threshold var legs = new SimpleBodyRestriction(body => { BoolExpr expr1 = Z3.Context.MkLt(body.Joints[JointType.KneeLeft].Y, Z3Math.Real(0)); BoolExpr expr2 = Z3.Context.MkLt(body.Joints[JointType.KneeRight].Y, Z3Math.Real(0)); BoolExpr expr = Z3.Context.MkAnd(expr1, expr2); return(expr); }); result.And(legs); // Ankles // Make sure ankles are not inclinated up more than knees // unless ankles are pointing back // TODO return(result); }