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; } }
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; } }