/// <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 CompositeBodyTransform CrossoverArmStretchTransform(JointSide sideOfStretchedArm)
{
JointSide oppositeSide;
JointTransform stretchDirectionTransform;
// First set the stretch direction and the opposite side (helper arm side)
if (sideOfStretchedArm == JointSide.Left)
{
oppositeSide = JointSide.Right;
stretchDirectionTransform = new SetJointDirectionTransform(1, 0, 0);
}
else
{
oppositeSide = JointSide.Left;
stretchDirectionTransform = new SetJointDirectionTransform(-1, 0, 0);
}
// Get all joint types for both stretched and support arm
JointType stretchedElbow = JointTypeHelper.GetSidedJointType(SidedJointName.Elbow, sideOfStretchedArm);
JointType stretchedWrist = JointTypeHelper.GetSidedJointType(SidedJointName.Wrist, sideOfStretchedArm);
JointType supportElbow = JointTypeHelper.GetSidedJointType(SidedJointName.Elbow, oppositeSide);
JointType supportWrist = JointTypeHelper.GetSidedJointType(SidedJointName.Wrist, oppositeSide);
// Apply the transforms in both arms
return(new CompositeBodyTransform()
.Compose(stretchedElbow, stretchDirectionTransform)
.Compose(stretchedWrist, stretchDirectionTransform)
.Compose(supportElbow, new SetJointDirectionTransform(0, -1, 0))
.Compose(supportWrist, new SetJointDirectionTransform(0, 1, 0)));
}
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false)
: base(body =>
{
JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
Z3Point3D joint1Position = body.GetJointPosition(jointType);
Z3Point3D joint2Position = body.GetJointPosition(sidedHandType);
BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold);
if (dont)
{
expr = Z3.Context.MkNot(expr);
}
return(expr);
},
jointType,
JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide))
{
this.JointType = jointType;
this.HandSide = handSide;
if (dont)
{
isNegated = true;
}
else
{
isNegated = false;
}
}
internal static Z3Body CreateDefaultZ3Body()
{
var joints = JointTypeHelper.CreateSyntheticJoints();
var norms = JointTypeHelper.CreateSyntheticNorms();
var vectors = JointTypeHelper.CreateSyntheticVectors();
var positions = JointTypeHelper.CreateSyntheticPositions();
return(new Z3Body(positions, vectors, joints, norms));
}
private static void SumWithFatherPosition(
Dictionary <Microsoft.Kinect.JointType, Vector3D> jointVectors,
List <PreposeGestures.JointType> targetCalculatedJoints,
Microsoft.Kinect.JointType jointType)
{
if (targetCalculatedJoints.Contains((PreposeGestures.JointType)jointType))
{
jointVectors[jointType] +=
jointVectors[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather((PreposeGestures.JointType)jointType)];
}
}
public override string ToString()
{
var result = string.Format("align your {0} and your {1}",
JointTypeHelper.JointToString(this.JointType1),
JointTypeHelper.JointToString(this.JointType2));
if (this.isNegated)
{
result = "don't " + result;
}
return(result);
}
public override string ToString()
{
var result = string.Format("touch your {0} with your {1} hand",
JointTypeHelper.JointToString(this.JointType),
EnumUtil.GetDescription <JointSide>(this.HandSide).ToLower());
if (this.isNegated)
{
result = "don't " + result;
}
return(result);
}
public override string ToString()
{
var result = string.Format("put your {0} {1} {2}",
JointTypeHelper.JointToString(this.JointType1),
EnumUtil.GetDescription <RelativeDirection>(this.Direction).ToLower(),
JointTypeHelper.JointToString(this.JointType2));
if (this.isNegated)
{
result = "don't " + result;
}
return(result);
}
public TouchBodyRestriction(JointType jointType, JointSide handSide, double distanceThreshold = 0.2, bool dont = false)
: base(body =>
{
JointType sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
Z3Point3D joint1Position = body.GetJointZ3Position(jointType);
Z3Point3D joint2Position = body.GetJointZ3Position(sidedHandType);
BoolExpr expr = joint1Position.IsNearerThan(joint2Position, distanceThreshold);
if (dont)
{
expr = Z3.Context.MkNot(expr);
}
return(expr);
},
body =>
{
var stopwatch = new Stopwatch();
stopwatch.Start();
var sidedHandType = JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide);
var p1 = body.Positions[jointType];
var p2 = body.Positions[sidedHandType];
var distance = Math.Max(0.00000001, p1.DistanceTo(p2));
var percentage = Math.Min(1.0, distanceThreshold / distance);
if (dont)
{
percentage = Math.Min(1.0, distance / distanceThreshold);
}
//Console.WriteLine("tch " + stopwatch.ElapsedTicks);
return(percentage);
},
jointType,
JointTypeHelper.GetSidedJointType(SidedJointName.Hand, handSide))
{
this.JointType = jointType;
this.HandSide = handSide;
if (dont)
{
isNegated = true;
}
else
{
isNegated = false;
}
}
//public Dictionary<JointType, double> GrabDistances(Z3Body that, List<JointType> joints)
//{
// var distanceExprs = this.GrabDistancePoint3Ds(that, joints);
// var result = new Dictionary<JointType, double>();
// foreach (var jointType in joints)
// result.Add(jointType, Z3Math.GetRealValue(distanceExprs[jointType]));
// return result;
//}
//public List<JointType> GetList
public Z3Point3D GetJointZ3Position(JointType jointType)
{
Z3Point3D result = new Z3Point3D(0, 0, 0);
JointType currentJointType = jointType;
// SpineBase is the root of the coordinate system
while (currentJointType != JointType.SpineBase)
{
result = result + (this.Joints[currentJointType] * this.Norms[currentJointType]);
currentJointType = JointTypeHelper.GetFather(currentJointType);
}
return(result);
}
/// <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);
}
/// <summary>
/// Checks if the pose is within default safety
/// restrictions when the transform and restrictions
/// are applied.
/// </summary>
/// <returns>True if it's safe</returns>
public static bool IsWithinSafetyRestrictions(Pose pose, out Z3Body witness)
{
Z3Body input = Z3Body.MkZ3Const();
Z3Body transformed = pose.Transform.Transform(input);
IBodyRestriction safe = Safety.DefaultSafetyRestriction();
BoolExpr inputSafe = safe.Evaluate(input);
BoolExpr transformedRestricted = pose.Restriction.Evaluate(transformed);
// Try to generate a unsafe witness using the transform
BoolExpr outputUnsafe = Z3.Context.MkNot(safe.Evaluate(transformed));
// Put together all expressions and search for unsat
BoolExpr expr = Z3.Context.MkAnd(inputSafe, transformedRestricted, outputUnsafe);
SolverCheckResult solverResult = Z3AnalysisInterface.CheckStatus(expr);
if (solverResult.Status == Status.SATISFIABLE)
{
//Z3Body
witness =
Z3AnalysisInterface.CreateBodyWitness(
transformed,
solverResult.Model,
pose.GetAllJointTypes(),
JointTypeHelper.CreateDefaultZ3Body());
return(false);
}
else if (solverResult.Status == Status.UNKNOWN)
{
//Z3Body
witness = JointTypeHelper.CreateDefaultZ3Body();
return(false);
}
else
{
Contract.Assert(solverResult.Status == Status.UNSATISFIABLE);
witness = null;
return(true);
}
}
private static void AddZ3JointToVectors3D(
Z3Body targetBody,
IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint> baseJoints,
Dictionary <Microsoft.Kinect.JointType, Vector3D> jointVectors,
PreposeGestures.JointType jointType)
{
var vector3D = new Vector3D(
targetBody.Joints[jointType].GetXValue(),
targetBody.Joints[jointType].GetYValue(),
-targetBody.Joints[jointType].GetZValue());
//var norm = Z3Math.GetRealValue(targetBody.Norms[jointType]);
var norm =
new Vector3D(
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.X,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Y,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z - baseJoints[(Microsoft.Kinect.JointType)JointTypeHelper.GetFather(jointType)].Position.Z).Length;
vector3D = vector3D * norm;
jointVectors.Add((Microsoft.Kinect.JointType)jointType, vector3D);
}
public override string ToString()
{
return(string.Format("rotate your {0} {1} degrees {2}",
JointTypeHelper.JointToString(this.JointType), this.Degrees,
EnumUtil.GetDescription <Direction>(this.Direction).ToLower()));
}
/// <summary>
/// Check for pairwise ambiguity
/// </summary>
/// <returns></returns>
public static bool HasPairwiseConflicts(App app, out List <PairwiseConflictException> allExceptions, out List <AmbiguityTime> ambiguityTimes, int precision = 15)
{
List <Gesture> conflictGestures = (List <Gesture>)app.Gestures;
bool result = false;
allExceptions = new List <PairwiseConflictException>();
ambiguityTimes = new List <AmbiguityTime>();
for (int i = 0; i < conflictGestures.Count - 1; i++)
{
for (int j = i + 1; j < conflictGestures.Count; j++)
{
var gesture1 = conflictGestures[i];
var gesture2 = conflictGestures[j];
// Create const input body
var input = Z3Body.MkZ3Const();
var allJoints = EnumUtil.GetValues <JointType>().ToList();
Z3Body transformed1 = null;
Z3Body transformed2 = null;
BoolExpr evaluation1 = Z3Math.True;
BoolExpr evaluation2 = Z3Math.True;
// Pass input through both gestures
gesture1.FinalResult(input, out transformed1, out evaluation1);
gesture2.FinalResult(input, out transformed2, out evaluation2);
// Check if it is possible that both outputs are equals
// This is performed by checking if is possible that all expressions are true
var isNearExpr = transformed1.IsNearerThan(
transformed2, precision);
var expr = Z3.Context.MkAnd(isNearExpr, evaluation1, evaluation2);
// If we are dumping Z3 constraints, then convert the expression to a SMTLIB formatted string
// and dump it to disk. Note this is not included in the timing for the individual pair of gestures,
// but it _is_ included in the timing for the app overall.
if (DumpZ3Constraints)
{
string exprName = String.Join("X", gesture1.Name, gesture2.Name);
string exprPath = exprName + ".smt2";
Z3AnalysisInterface.WriteExprToDisk(expr, exprName, exprPath);
}
// Check if we have an ambiguity conflict. Record the time it takes.
var stopwatch = new Stopwatch();
stopwatch.Start();
var checkResult = Z3AnalysisInterface.CheckStatus(expr);
stopwatch.Stop();
if (checkResult.Status == Status.SATISFIABLE)
{
var witness = Z3AnalysisInterface.CreateBodyWitness(
input,
checkResult.Model,
allJoints,
JointTypeHelper.CreateDefaultZ3Body());
var exception = new PairwiseConflictException(
"Conflict detected between pair of gestures",
gesture1,
gesture2,
witness);
allExceptions.Add(exception);
result = true;
}
// TODO the witness here should exist, this case shouldn't be needed
else if (checkResult.Status == Status.UNKNOWN)
{
var witness = JointTypeHelper.CreateDefaultZ3Body();
var exception = new PairwiseConflictException(
"Conflict detected between pair of gestures, the reason is unknown",
gesture1,
gesture2,
witness);
allExceptions.Add(exception);
result = true;
}
ambiguityTimes.Add(new AmbiguityTime
{
Gesture1 = gesture1,
Gesture2 = gesture2,
Time = stopwatch.ElapsedMilliseconds,
Conflict = result,
CheckResult = checkResult
});
}
}
return(result);
}
internal static Z3Body CreateDefaultZ3Body()
{
return(new Z3Body(JointTypeHelper.CreateSyntheticJoints(), JointTypeHelper.CreateSyntheticNorms()));
}
