/// <summary>
/// Produces a witness body given input restrictions, output
/// restrictions and a transform.
/// </summary>
/// <param name="inputBodyRestriction"></param>
/// <param name="bodyTransform"></param>
/// <param name="outputBodyRestriction"></param>
/// <returns></returns>
public static Z3Body GenerateWitness(
CompositeBodyRestriction inputBodyRestriction,
BodyTransform bodyTransform,
CompositeBodyRestriction outputBodyRestriction)
{
var body = Z3Body.MkZ3Const();
var transformedBody = bodyTransform.Transform(body);
var expr1 = inputBodyRestriction.Evaluate(body);
var expr2 = outputBodyRestriction.Evaluate(transformedBody);
var expr = Z3.Context.MkAnd(expr1, expr2);
var evaluatedJoints =
JointTypeHelper.MergeJointTypeLists(
inputBodyRestriction.GetJointTypes(),
bodyTransform.GetJointTypes(),
outputBodyRestriction.GetJointTypes());
var checkResult = CheckStatus(expr);
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = CreateBodyWitness(
body,
checkResult.Model,
evaluatedJoints,
JointTypeHelper.CreateDefaultZ3Body());
return(witness);
}
else
{
return(null);
}
}
public Pose(string name)
{
this.Name = name;
this.mTransform = new CompositeBodyTransform();
this.mRestriction = new CompositeBodyRestriction();
this.Delayed = new CompositeDelayedStatement();
}
public static bool IsWithinDefaultSafetyRestrictions(Pose pose, out string firstBadStatement)
{
bool result = true;
firstBadStatement = "";
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
var restrictions = Safety.DefaultSafetyRestriction().Restrictions;
var composite = new CompositeBodyRestriction();
foreach (var restriction in restrictions)
{
composite.And(restriction);
BoolExpr inputSafe = composite.Evaluate(transformed);
BoolExpr expr = Z3.Context.MkAnd(transformedRestricted, inputSafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.UNSATISFIABLE)
{
firstBadStatement = ((SimpleBodyRestriction)restriction).Message;
result = false;
break;
}
}
return(result);
}
public static bool IsInternallyValid(Pose pose, out string firstBadStatement)
{
bool result = true;
firstBadStatement = "";
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
var restrictions = pose.Restriction.Restrictions;
var composite = new CompositeBodyRestriction();
foreach (var restriction in restrictions)
{
composite.And(restriction);
BoolExpr transformedRestricted = composite.Evaluate(transformed);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(transformedRestricted);
if (solverResult.Status == Status.UNSATISFIABLE)
{
firstBadStatement = restriction.ToString();
result = false;
break;
}
}
return(result);
}
public CompositeBodyRestriction Aggregate(Func <JointType, IBodyRestriction> func)
{
var result = new CompositeBodyRestriction();
foreach (var joint in this.Joints)
{
var that = func(joint);
result = result.And(that);
}
return(result);
}
public Pose(string name, CompositeBodyTransform transform, IBodyRestriction restriction)
: this(name, transform)
{
this.mRestriction = (restriction is CompositeBodyRestriction) ?
(CompositeBodyRestriction)restriction :
new CompositeBodyRestriction((SimpleBodyRestriction)restriction);
// Check if restriction allows transform
if (!this.IsTransformAcceptedByRestriction())
{
throw new ArgumentException("The restriction does not allow the transform.", "restriction");
}
}
public CompositeBodyRestriction And(IBodyRestriction that)
{
CompositeBodyRestriction result = new CompositeBodyRestriction();
result.Restrictions.AddRange(this.Restrictions);
if (that is CompositeBodyRestriction)
{
result.Restrictions.AddRange(((CompositeBodyRestriction)that).Restrictions);
}
else
{
result.Restrictions.Add(((SimpleBodyRestriction)that));
}
return(result);
}
// Generates a body witness which satisfies two conditions
// 1. It is within a range (angle threshold) of a transform from a start body
// 2. It is within the considered restrictions
public static Z3Target GenerateTarget(
BodyTransform transform,
CompositeBodyRestriction restriction,
Z3Body startBody,
int angleThreshold)
{
var z3ConstBody = Z3Body.MkZ3Const();
z3ConstBody.Norms = startBody.Norms;
var transformedBody = transform.Transform(startBody);
var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList();
var isNearExpr = z3ConstBody.IsAngleBetweenLessThan(transformedBody, joints, angleThreshold);
var evaluateExpr = restriction.Evaluate(z3ConstBody);
//var normsExpr = BodyRestrictionBuilder.EvaluateNorms(startBody, z3ConstBody);
//var expr = Z3.Context.MkAnd(isNearExpr, evaluateExpr, normsExpr);
//var expr = Z3.Context.MkAnd(evaluateExpr, normsExpr);
var expr = Z3.Context.MkAnd(evaluateExpr, isNearExpr);
var checkResult = CheckStatus(expr);
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = CreateBodyWitness(
z3ConstBody,
checkResult.Model,
restriction.GetJointTypes(),
startBody);
var target = new Z3Target();
target.Body = witness;
target.RestrictedJoints = restriction.GetJointTypes();
target.TransformedJoints = transform.GetJointTypes();
foreach (var jointType in transform.GetJointTypes())
{
target.Body.Joints[jointType] = transformedBody.Joints[jointType];
}
return(target);
}
else
{
return(null);
}
}
/// <summary>
/// Produces a witness body given input restrictions, output
/// restrictions and a transform.
/// </summary>
/// <param name="inputBodyRestriction"></param>
/// <param name="bodyTransform"></param>
/// <param name="outputBodyRestriction"></param>
/// <returns></returns>
public static Z3Body GenerateWitness(
CompositeBodyRestriction inputBodyRestriction,
BodyTransform bodyTransform,
CompositeBodyRestriction outputBodyRestriction)
{
var body = Z3Body.MkZ3Const();
var transformedBody = bodyTransform.Transform(body);
var expr1 = inputBodyRestriction.Evaluate(body);
var expr2 = outputBodyRestriction.Evaluate(transformedBody);
var expr = Z3.Context.MkAnd(expr1, expr2);
var evaluatedJoints =
JointTypeHelper.MergeJointTypeLists(
inputBodyRestriction.GetJointTypes(),
bodyTransform.GetJointTypes(),
outputBodyRestriction.GetJointTypes());
var checkResult = CheckStatus(expr);
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = CreateBodyWitness(
body,
checkResult.Model,
evaluatedJoints,
JointTypeHelper.CreateDefaultZ3Body());
return witness;
}
else
{
return null;
}
}
// Generates a body witness which satisfies two conditions
// 1. It is within a range (angle threshold) of a transform from a start body
// 2. It is within the considered restrictions
public static Z3Target GenerateTarget(
BodyTransform transform,
CompositeBodyRestriction restriction,
Z3Body startBody,
int angleThreshold)
{
var z3ConstBody = Z3Body.MkZ3Const();
z3ConstBody.Norms = startBody.Norms;
var transformedBody = transform.Transform(startBody);
var joints = transform.GetJointTypes().Union(restriction.GetJointTypes()).ToList();
var isNearExpr = z3ConstBody.IsAngleBetweenLessThan(transformedBody, joints, angleThreshold);
var evaluateExpr = restriction.Evaluate(z3ConstBody);
//var normsExpr = BodyRestrictionBuilder.EvaluateNorms(startBody, z3ConstBody);
//var expr = Z3.Context.MkAnd(isNearExpr, evaluateExpr, normsExpr);
//var expr = Z3.Context.MkAnd(evaluateExpr, normsExpr);
var expr = Z3.Context.MkAnd(evaluateExpr, isNearExpr);
var checkResult = CheckStatus(expr);
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = CreateBodyWitness(
z3ConstBody,
checkResult.Model,
restriction.GetJointTypes(),
startBody);
var target = new Z3Target();
target.Body = witness;
target.RestrictedJoints = restriction.GetJointTypes();
target.TransformedJoints = transform.GetJointTypes();
foreach (var jointType in transform.GetJointTypes())
target.Body.Joints[jointType] = transformedBody.Joints[jointType];
return target;
}
else
{
return null;
}
}
/// <summary>
/// This is a basic safety check to make sure we don't break any bones.
/// </summary>
/// <returns>True if safe</returns>
public static CompositeBodyRestriction 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].IsDegreesBetweenLessThan(up, inclinationThreshold);
// BoolExpr expr2 = body.Joints[JointType.Neck].IsDegreesBetweenLessThan(up, inclinationThreshold);
// BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
// return expr;
//},
//body => { return 1.0; });
//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].IsDegreesBetweenLessThan(up, inclinationThreshold);
// BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsDegreesBetweenLessThan(up, inclinationThreshold);
// BoolExpr expr3 =
// body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(
// body.Joints[JointType.SpineShoulder], inclinationThreshold);
// BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
// return expr;
//},
//body => { return 1.0; });
//result.And(spine);
//// Shoulders
//// Make sure shoulders are not bent
//var shoulders = new SimpleBodyRestriction(body =>
//{
// BoolExpr expr =
// body.Joints[JointType.SpineMid].IsDegreesBetweenGreaterThan(
// body.Joints[JointType.SpineShoulder], 120);
// return expr;
//},
//body => { return 1.0; });
//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 elbows1 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.ElbowRight);
var joint2Position = body.GetJointZ3Position(JointType.ShoulderRight);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkGt(joint1Position.Y, joint2Position.Y)));
return(expr);
}, body => { return(1.0); },
"don't put your right elbow behind you and above your shoulders.\n");
var elbows2 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.ElbowLeft);
var joint2Position = body.GetJointZ3Position(JointType.ShoulderLeft);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkGt(joint1Position.Y, joint2Position.Y)));
return(expr);
}, body => { return(1.0); },
"don't put your left elbow behind you and above your shoulders.\n");
var elbows3 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.ElbowRight);
var joint2Position = body.GetJointZ3Position(JointType.ShoulderRight);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkLt(joint1Position.X, joint2Position.X)));
return(expr);
}, body => { return(1.0); },
"don't put your right elbow behind you crossing your back.\n");
var elbows4 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.ElbowLeft);
var joint2Position = body.GetJointZ3Position(JointType.ShoulderLeft);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkGt(joint1Position.X, joint2Position.X)));
return(expr);
}, body => { return(1.0); },
"don't put your right elbow behind you crossing your back.\n");
result.And(elbows1);
result.And(elbows2);
result.And(elbows3);
result.And(elbows4);
//// 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.IsDegreesBetweenLessThan(hipsSum, 45);
// return expr;
//},
//body => { return 1.0; });
//result.And(hips);
// Legs
// Make sure knees do not bent towards your back
var knees1 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.KneeLeft);
var joint2Position = body.GetJointZ3Position(JointType.HipLeft);
var joint3Position = body.GetJointZ3Position(JointType.AnkleLeft);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkLt(joint1Position.Z, joint3Position.Z)));
return(expr);
}, body => { return(1.0); },
"don't bend your left knee towards your back.\n");
var knees2 = new SimpleBodyRestriction(body =>
{
var joint1Position = body.GetJointZ3Position(JointType.KneeRight);
var joint2Position = body.GetJointZ3Position(JointType.HipRight);
var joint3Position = body.GetJointZ3Position(JointType.AnkleRight);
BoolExpr expr = Z3.Context.MkNot(Z3.Context.MkAnd(
Z3.Context.MkLt(joint1Position.Z, joint2Position.Z),
Z3.Context.MkLt(joint1Position.Z, joint3Position.Z)));
return(expr);
}, body => { return(1.0); },
"don't bend your right knee towards your back.\n");
result.And(knees1);
result.And(knees2);
// Ankles
// Make sure ankles are not inclinated up more than knees
// unless ankles are pointing back
// TODO
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].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.Neck].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2);
return expr;
},
body => { return 1.0; });
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].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr2 = body.Joints[JointType.SpineShoulder].IsDegreesBetweenLessThan(up, inclinationThreshold);
BoolExpr expr3 =
body.Joints[JointType.SpineMid].IsDegreesBetweenLessThan(
body.Joints[JointType.SpineShoulder], inclinationThreshold);
BoolExpr expr = Z3.Context.MkAnd(expr1, expr2, expr3);
return expr;
},
body => { return 1.0; });
result.And(spine);
// Shoulders
// Make sure shoulders are not bent
var shoulders = new SimpleBodyRestriction(body =>
{
BoolExpr expr =
body.Joints[JointType.SpineMid].IsDegreesBetweenGreaterThan(
body.Joints[JointType.SpineShoulder], 120);
return expr;
},
body => { return 1.0; });
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;
},
body => { return 1.0; });
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.IsDegreesBetweenLessThan(hipsSum, 45);
return expr;
},
body => { return 1.0; });
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;
},
body => { return 1.0; });
result.And(legs);
// Ankles
// Make sure ankles are not inclinated up more than knees
// unless ankles are pointing back
// TODO
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);
}
public CompositeBodyRestriction And(IBodyRestriction that)
{
CompositeBodyRestriction result = new CompositeBodyRestriction();
result.Restrictions.AddRange(this.Restrictions);
if (that is CompositeBodyRestriction)
{
result.Restrictions.AddRange(((CompositeBodyRestriction)that).Restrictions);
}
else
{
result.Restrictions.Add(((SimpleBodyRestriction)that));
}
return result;
}