public Z3Body(Z3Body baseBody) : this()
{
foreach (var jointType in baseBody.Joints.Keys)
{
this.Positions.Add(jointType,
new Point3D(
baseBody.Positions[jointType].X,
baseBody.Positions[jointType].Y,
baseBody.Positions[jointType].Z));
this.Vectors.Add(jointType,
new Point3D(
baseBody.Vectors[jointType].X,
baseBody.Vectors[jointType].Y,
baseBody.Vectors[jointType].Z));
this.Joints.Add(jointType,
new Z3Point3D(
Z3Math.GetRealValue(baseBody.Joints[jointType].X),
Z3Math.GetRealValue(baseBody.Joints[jointType].Y),
Z3Math.GetRealValue(baseBody.Joints[jointType].Z)
));
this.Norms.Add(jointType, Z3Math.Real(Z3Math.GetRealValue(baseBody.Norms[jointType])));
}
}
/// <summary>
/// Apply this transform to obtain an output body.
/// </summary>
/// <param name="inputBody"></param>
/// <returns></returns>
public Z3Body Transform(Z3Body inputBody)
{
var outputBody = new Z3Body();
var jointTypes = EnumUtil.GetValues <JointType>();
foreach (var jointType in jointTypes)
{
// Both this and inputBody have the jointType, so transform the joint
if (this.JointTransforms.ContainsKey(jointType) &&
inputBody.Joints.ContainsKey(jointType))
{
outputBody.Joints.Add(jointType,
this.JointTransforms[jointType].Transform(
inputBody.Joints[jointType]));
outputBody.Norms.Add(jointType,
inputBody.Norms[jointType]);
}
// The joint exists in body but there is no transform for it, so add it
else if (inputBody.Joints.ContainsKey(jointType))
{
outputBody.Joints.Add(jointType,
inputBody.Joints[jointType]);
outputBody.Norms.Add(jointType,
inputBody.Norms[jointType]);
}
// Do nothing otherwise
}
return(outputBody);
}
public bool IsBodyAccepted(
Z3Body input)
{
bool result = this.Restriction.IsBodyAccepted(input);
return(result);
}
public bool IsBodyAccepted(Z3Body body, Context localContext)
{
BoolExpr resultExpr = Evaluate(body);
bool result = Z3.EvaluateBoolExpr(resultExpr, localContext);
return(result);
}
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);
}
/// <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 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);
}
//internal static SimpleBodyRestriction TouchRestriction(JointType jointType, JointSide handSide)
//{
// double maxY = TrigonometryHelper.GetSine(5);
// return new TouchBodyRestriction(jointType, handSide);
//}
internal static BoolExpr EvaluateNorms(Z3Body body1, Z3Body body2)
{
double normsThreshold = 0.1;
BoolExpr result = Z3Math.True;
var jointTypes = EnumUtil.GetValues <JointType>();
foreach (var jointType in jointTypes)
{
// Calc the distance between the two norms
ArithExpr distance =
Z3Math.Abs(
Z3Math.Sub(
body1.Norms[jointType],
body2.Norms[jointType]));
// Create the boolean expression to evaluate the distance
result =
Z3.Context.MkAnd(
result,
Z3.Context.MkLt(
distance,
Z3Math.Real(normsThreshold)));
}
return(result);
}
private void Reset(Z3Body body)
{
this.CurrentStep = 0;
//this.StepLastPercentage = 0;
this.UpdateTargetBody(body);
this.AccumulatedError = 0;
this.Target = null;
}
public void InitBody(Z3Body body, Context localContext)
{
GestureMatcherLocalContext = localContext;
if (this.Target == null)
{
this.UpdateTargetBody(body);
}
}
private void Reset(Z3Body body)
{
this.CurrentStep = 0;
this.LastDistance = 0;
this.UpdateTargetBody(body);
this.AccumulatedError = 0;
this.Target = null;
}
public bool IsTransformAcceptedByRestriction()
{
Z3Body body = Z3Body.MkZ3Const();
Z3Body transformedBody = this.Transform.Transform(body);
BoolExpr expr = this.Restriction.Evaluate(transformedBody);
SolverCheckResult checkResult = Z3AnalysisInterface.CheckStatus(expr);
return(checkResult.Status != Status.UNSATISFIABLE);
}
public bool IsTransformedBodyAccepted(Z3Body body)
{
bool result = false;
Z3Body transformedBody = this.mTransform.Transform(body);
result = this.IsBodyAccepted(transformedBody);
return(result);
}
public Dictionary <JointType, Z3Point3D> GrabDistancePoint3Ds(Z3Body that, List <JointType> joints)
{
var result = new Dictionary <JointType, Z3Point3D>();// Z3Math.Zero;
foreach (var jointType in joints)
{
result.Add(jointType, this.Joints[jointType].GrabDistancePoint3D(that.Joints[jointType]));
}
return(result);
}
private static BoolExpr ExpressionFromCurrentStep(Gesture gesture1, int gesture1CurrentStep)
{
var input1 = Z3Body.MkZ3Const();
var step1 = gesture1.Steps[gesture1CurrentStep];
var pose1 = step1.Pose;
input1 = pose1.Transform.Transform(input1);
var expr1 = pose1.Restriction.Evaluate(input1);
return(expr1);
}
public void FinalResult(Z3Body input, out Z3Body transformed, out BoolExpr evaluation)
{
transformed = input;
evaluation = Z3Math.True;
foreach (var step in this.Steps)
{
var pose = step.Pose;
transformed = pose.Transform.Transform(transformed);
evaluation = Z3.Context.MkAnd(evaluation, pose.Restriction.Evaluate(transformed));
}
}
public BoolExpr IsNearerThan(Z3Body that, double distanceThreshold)
{
BoolExpr result = Z3.Context.MkTrue();
var jointTypes = EnumUtil.GetValues <JointType>();
foreach (var jointType in jointTypes)
{
result = Z3.Context.MkAnd(this.Joints[jointType].IsNearerThan(that.Joints[jointType], distanceThreshold));
}
return(result);
}
public static Z3Body MkZ3Const()
{
Z3Body result = new Z3Body();
var jointTypes = EnumUtil.GetValues <JointType>();
foreach (var jointType in jointTypes)
{
result.Joints.Add(jointType, Z3Point3D.MkZ3Const(jointType.ToString()));
result.Norms.Add(jointType, Z3.Context.MkRealConst(jointType.ToString() + " Norm"));
}
return(result);
}
public Z3Body(Z3Body baseBody)
{
foreach (var jointType in baseBody.Joints.Keys)
{
this.Joints.Add(jointType,
new Z3Point3D(
Z3Math.GetRealValue(baseBody.Joints[jointType].X),
Z3Math.GetRealValue(baseBody.Joints[jointType].Y),
Z3Math.GetRealValue(baseBody.Joints[jointType].Z)
));
this.Norms.Add(jointType, Z3Math.Real(Z3Math.GetRealValue(baseBody.Norms[jointType])));
}
}
/// <summary>
/// Generates a witness body tha matches the restrictions that
/// are passed in.
/// </summary>
/// <param name="bodyRestriction"></param>
/// <returns></returns>
public static Z3Body GenerateWitness(IBodyRestriction bodyRestriction)
{
var body = Z3Body.MkZ3Const();
var expr = bodyRestriction.Evaluate(body);
var checkResult = CheckStatus(expr);
var witness = CreateBodyWitness(
body,
checkResult.Model,
bodyRestriction.GetJointTypes(),
JointTypeHelper.CreateDefaultZ3Body());
return(witness);
}
public Z3Target CalcNearestTargetBody(Z3Body startBody)
{
// Create binary search to look for nearest body
int numSteps = 6;
int angleThreshold = 90;
int angleIncrement = 90;
Z3Target target = null;
for (int i = 0; i < numSteps; ++i)
{
// Ask for a witness which is within the range
target = Z3AnalysisInterface.GenerateTarget(
this.Transform,
this.Restriction,
startBody,
angleThreshold);
// Update angle threshold
angleIncrement /= 2;
if (target != null)
{
angleThreshold -= angleIncrement;
}
else
{
angleThreshold += angleIncrement;
}
}
// If target still null it probably means Z3 was unable to solve the restrictions
// This way we generate a target using onlye the transforms
if (target == null)
{
target = new Z3Target();
target.Body = this.Transform.Transform(startBody);
target.TransformedJoints = this.Transform.GetJointTypes();
}
// If target still null assing a new body as an error proof policy
if (target == null)
{
target = new Z3Target();
target.Body = startBody;
}
return(target);
}
public BoolExpr Evaluate(Z3Body body)
{
Func <Z3Body, BoolExpr> composedExpr = bodyInput =>
{
BoolExpr result = Z3.Context.MkTrue();
foreach (var restriction in this.Restrictions)
{
result = Z3.Context.MkAnd(result, restriction.Evaluate(bodyInput));
}
return(result);
};
return(composedExpr(body));
}
//public Dictionary<JointType, ArithExpr> CalcDistanceExprs(Z3Target target)
//{
// return CalcDistanceExprs(target.Body, target.TransformedJoints);
//}
public BoolExpr IsDegreesBetweenLessThan(Z3Body that, List <JointType> joints, int degreesThreshold)
{
BoolExpr result = Z3Math.True;
foreach (var jointType in joints)
{
// Only add distance if the target (that) body has the joint active
// This allows partial matching, evaluating only the needed joints for the pose
result =
Z3.Context.MkAnd(
result,
this.Joints[jointType].IsDegreesBetweenLessThan(that.Joints[jointType], degreesThreshold));
}
return(result);
}
// Calculates average joint distance to target body considering only the active joints
public Dictionary <JointType, ArithExpr> CalcDistanceExprs(Z3Body that, List <JointType> joints)
{
var result = new Dictionary <JointType, ArithExpr>();// Z3Math.Zero;
foreach (var jointType in joints)
{
// Only add distance if the target (that) body has the joint active
// This allows partial matching, evaluating only the needed joints for the pose
//result = Z3Math.Add(result, this.Joints[jointType].CalcApproximateDistance(that.Joints[jointType]));
//result = Z3Math.Max(result, this.Joints[jointType].CalcApproximateDistance(that.Joints[jointType]));
result.Add(jointType, this.Joints[jointType].CalcApproximateDistance(that.Joints[jointType]));
}
//result = Z3Math.Div(result, Z3Math.Real(joints.Count));
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);
}
}
public bool IsBodyAccepted(Z3Body body)
{
BoolExpr resultExpr = RestrictionFunc(body);
Solver solver = Z3.Context.MkSolver();
solver.Push();
solver.Assert(Z3.Context.MkNot(resultExpr));
Status status = solver.Check();
Statistics stats = solver.Statistics;
bool result = (status == Status.UNSATISFIABLE);
solver.Pop();
return(result);
}
internal void UpdateTargetBody(Z3Body startBody)
{
// delayed statements are a special case
// they can be treated as restrictions or as transformations
// in order to the pose to retrieve the composite restrictions and transformations
// we must first provide the start body to update its delayed statements
// so they are treated first on the update target function
this.Gesture.Steps[this.CurrentStep].Pose.UpdateDelayedStatements(startBody);
var stopwatch = new Stopwatch();
StatisticsEntrySynthesize synTime = new StatisticsEntrySynthesize();
stopwatch.Start();
var time0 = stopwatch.ElapsedMilliseconds;
if (this.Target != null && this.CurrentStep > 0)
{
// If last target is not null than use the target transformed joints instead of the start transformed joints
// This way no error is cumulated along the transforms due to the matching precisions
foreach (var jointType in this.Gesture.Steps[this.CurrentStep - 1].Pose.GetTransformJointTypes())
{
// The Z3Point3D depends on a specific Z3 context deep underneath
// We need to thread the context through
startBody.Joints[jointType] =
new Z3Point3D(
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].X),
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Y),
Z3Math.GetRealValue(this.Target.Body.Joints[jointType].Z),
Z3.Context);
}
}
this.Target = this.Gesture.Steps[this.CurrentStep].Pose.CalcNearestTargetBody(startBody);
this.LastDistanceVectors = UpdateDistanceVectors(startBody, Z3.Context);
var time1 = stopwatch.ElapsedMilliseconds - time0;
stopwatch.Stop();
synTime.timeMs = time1;
synTime.gestureName = this.Gesture.Name;
synTime.poseName = this.Gesture.Steps[CurrentStep].Pose.Name;
synTime.uid = frameCount;
GestureStatistics.synthTimes.Add(synTime);
frameCount++;
}
private Dictionary <JointType, Point3D> UpdateDistanceVectors(Z3Body body, Context localContext)
{
var distancesZ3Point3Ds = body.GrabDistancePoint3Ds(this.Target.Body, this.Target.GetAllJointTypes());
var distancesVector3Ds = new Dictionary <JointType, Point3D>();
foreach (var pointZ3 in distancesZ3Point3Ds)
{
distancesVector3Ds.Add(
pointZ3.Key,
new Point3D(
Z3Math.GetRealValue(pointZ3.Value.X),
Z3Math.GetRealValue(pointZ3.Value.Y),
Z3Math.GetRealValue(pointZ3.Value.Z)));
}
return(distancesVector3Ds);
}
public BoolExpr IsEqualsTo(Z3Body that)
{
BoolExpr result = Z3Math.True;
var allJoints = EnumUtil.GetValues <JointType>();
foreach (var jointType in allJoints)
{
// Only add distance if the target (that) body has the joint active
// This allows partial matching, evaluating only the needed joints for the pose
result =
Z3.Context.MkAnd(
result,
this.Joints[jointType].IsEqualTo(that.Joints[jointType]));
}
return(result);
}
internal void Update(Z3Body body)
{
this.Restriction = new CompositeBodyRestriction();
this.Transform = new CompositeBodyTransform();
// treating rotate direction
// for each definition
// if a joint appears once
// restrict
// else
// transform
// the rotate direction can be a transform or a restriction
foreach (var definition in this.Statements)
{
var jointCount = 0;
foreach (var definitionTemp in this.Statements)
{
if (definition.JointType == definitionTemp.JointType)
{
jointCount++;
}
}
// if for a single joint there is only one rotate direction
// then the restriction should be applied to give more
// freedom for the user while performing the gesture
if (jointCount == 1)
{
this.Restriction.And(new RotateDirectionRestriction(
definition.JointType, body.Joints[definition.JointType],
definition.Degrees, definition.Direction));
}
// else if for that joint there is more than one rotate direction
// then we create a transform to guarantee that all directions
// are well represented
else if (jointCount > 1)
{
this.Transform = this.Transform.Compose(
new CompositeBodyTransform(definition.JointType,
new RotateJointTransform(definition.Degrees, definition.Direction)));
}
}
}
