public InterpOperation(ArmController arm, Vector3D worldtarg, double WristPitch, double WristYaw) { this.arm = arm; CurrentState = arm.GetCurrentState(); EndState = arm.FindAnglesState(worldtarg, WristPitch, WristYaw); EndPos = worldtarg; CurrentPos = arm.FindPos(out StartWristPitch, out StartWristYaw); //CurrentPos += arm.wristYaw.WorldMatrix.Up * arm.headHeight; stepAmount = stepSize / Vector3D.Distance(CurrentPos, EndPos); // StartWristPitch = arm.wristPitch.Angle + arm.shoulderPitch.Angle - arm.Elbow.Angle; // StartWristYaw = arm.wristYaw.Angle - arm.shoulderYaw.Angle; EndWristPitch = WristPitch; EndWristYaw = WristYaw; }