public static Z3Body CreateBodyWitness(
Z3Body z3ConstCheckedBody,
Model model,
List<JointType> evaluatedJoints,
Z3Body defaultBody)
{
var witness = new Z3Body();
var jointTypes = EnumUtil.GetValues<JointType>();
foreach (var jointType in jointTypes)
{
if (evaluatedJoints.Contains(jointType))
{
var joint = new Z3Point3D(
model.Evaluate(z3ConstCheckedBody.Joints[jointType].X, true) as ArithExpr,
model.Evaluate(z3ConstCheckedBody.Joints[jointType].Y, true) as ArithExpr,
model.Evaluate(z3ConstCheckedBody.Joints[jointType].Z, true) as ArithExpr);
witness.Joints.Add(jointType, joint);
var norm = model.Evaluate(z3ConstCheckedBody.Norms[jointType]) as ArithExpr;
// Check if norm is still an app (meaning it can be anything), then set it to be the default norm
if (norm.ASTKind == Z3_ast_kind.Z3_APP_AST)
witness.Norms.Add(jointType, defaultBody.Norms[jointType]);
else
witness.Norms.Add(jointType, norm);
}
else
{
witness.Joints.Add(jointType, defaultBody.Joints[jointType]);
witness.Norms.Add(jointType, defaultBody.Norms[jointType]);
}
}
return witness;
}