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])));
}
}
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])));
}
}
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);
}
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++;
}
internal void UpdateTargetBody(Z3Body start, Context localZ3Context)
{
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
start.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),
localZ3Context);
}
}
this.Target = this.Gesture.Steps[this.CurrentStep].Pose.CalcNearestTargetBody(start);
this.LastDistanceVectors = UpdateDistanceVectors(start, 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;
//totalZ3Time += time1;
frameCount++;
GestureStatistics.synthTimes.Add(synTime);
//Console.Write("z3time: " + (totalZ3Time / frameCount) + " \r");
}
//public ArithExpr CalcApproximateDistanceBetweenNormalized(Z3Point3D that)
//{
// Z3Point3D thisNormalized = this.GetApproximateNormalized();
// Z3Point3D thatNormalized = that.GetApproximateNormalized();
// ArithExpr result = thisNormalized.CalcApproximateDistance(thatNormalized);
// return result;
//}
// Assumes vectors are normalized
public BoolExpr IsAngleBetweenLessThan(Z3Point3D that, int angleThreshold)
{
double distanceThreshold = TrigonometryHelper.GetDistance(angleThreshold);
ArithExpr distance = this.CalcApproximateDistance(that);
BoolExpr result = Z3.Context.MkLt(distance, Z3Math.Real(distanceThreshold));
// TODO remove this, test code
SolverCheckResult checkResult = Z3AnalysisInterface.CheckStatus(result);
if (checkResult.Status == Status.SATISFIABLE)
{
var joint = new Z3Point3D(
checkResult.Model.Evaluate(this.X, true) as ArithExpr,
checkResult.Model.Evaluate(this.Y, true) as ArithExpr,
checkResult.Model.Evaluate(this.Z, true) as ArithExpr);
var distanceSolvedExpr = checkResult.Model.Evaluate(distance, true) as ArithExpr;
var distanceValue = Z3Math.GetRealValue(distanceSolvedExpr);
}
// end of test code
return(result);
}
public double GetZValue()
{
return(Z3Math.GetRealValue(Z));
}
public double GetYValue()
{
return(Z3Math.GetRealValue(Y));
}
public double GetXValue()
{
return(Z3Math.GetRealValue(X));
}
