/****************************************************************************** Copyright (c) 2008-2009 Ryan Juckett http://www.ryanjuckett.com/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. ******************************************************************************/ using System; using System.Collections.Generic; using System.ComponentModel; using System.Linq; using System.Text; using System.Windows; using System.Windows.Controls; using System.Windows.Data; using System.Windows.Documents; using System.Windows.Input; using System.Windows.Media; using System.Windows.Media.Imaging; using System.Windows.Navigation; using System.Windows.Shapes; namespace RJ_Demo_IK { /// /// 2D two bone analytic based inverse kinematics for all solutions /// public partial class CalcIK_2D_AllTwoBoneAnalytic : UserControl, INotifyPropertyChanged { #region Private data // bone lengths private double m_bone1_length = 100; private double m_bone2_length = 40; // bone angles (for both solutions) private double m_solution1_bone1_angle = 0; private double m_solution1_bone2_angle = 0; private double m_solution2_bone1_angle = 0; private double m_solution2_bone2_angle = 0; // target position to reach for private Point m_targetPos = new Point(0,0); // result of current IK calculation private bool m_validSolution = false; #endregion #region INotifyPropertyChanged interface // event used by the user interface to bind to our properties public event PropertyChangedEventHandler PropertyChanged; // helper function to notify PropertyChanged subscribers protected void NotifyPropertyChanged(string propertyName) { if (PropertyChanged != null) { PropertyChanged(this, new PropertyChangedEventArgs(propertyName)); } } #endregion #region Public properties public bool ValidSolution { get { return m_validSolution; } set { m_validSolution = value; NotifyPropertyChanged("ValidSolution"); // update the bound UI } } public double Bone1_Length { get { return m_bone1_length; } set { m_bone1_length = Math.Max(0,value); // only allow positive length values UpdateBonePositions(); // redraw the IK state } } public double Bone2_Length { get { return m_bone2_length; } set { m_bone2_length = Math.Max(0,value); // only allow positive length values UpdateBonePositions(); // redraw the IK state } } public double TargetPosX { get { return m_targetPos.X; } set { m_targetPos.X = value; NotifyPropertyChanged("TargetPosX"); // update the bound UI UpdateBonePositions(); // redraw the IK state } } public double TargetPosY { get { return m_targetPos.Y; } set { m_targetPos.Y = value; NotifyPropertyChanged("TargetPosY"); // update the bound UI UpdateBonePositions(); // redraw the IK state } } public double Solution1_Bone1_Degrees { get { return m_solution1_bone1_angle * 180.0 / Math.PI; } } public double Solution1_Bone2_Degrees { get { return m_solution1_bone2_angle * 180.0 / Math.PI; } } public double Solution2_Bone1_Degrees { get { return m_solution2_bone1_angle * 180.0 / Math.PI; } } public double Solution2_Bone2_Degrees { get { return m_solution2_bone2_angle * 180.0 / Math.PI; } } #endregion #region Lifespan functions public CalcIK_2D_AllTwoBoneAnalytic() { InitializeComponent(); // iniitalize the desired position to the end point of the bones TargetPosX = m_bone1_length * Math.Cos(m_solution1_bone1_angle) + m_bone2_length * Math.Cos(m_solution1_bone1_angle + m_solution1_bone2_angle); TargetPosY = m_bone1_length * Math.Sin(m_solution1_bone1_angle) + m_bone2_length * Math.Sin(m_solution1_bone1_angle + m_solution1_bone2_angle); // update the display UpdateBonePositions(); } #endregion #region Coordinate Conversion // compute the logical origin in _viewport coordinated private double ViewportWidth { get { return _viewportColumn.ActualWidth; } } private double ViewportHieght { get { return _mainGrid.ActualHeight; } } private double ViewportCenterX { get { return ViewportWidth / 2; } } private double ViewportCenterY { get { return ViewportHieght / 2; } } // convert logical coordinates to _viewport coordinates private double LogicalToViewportX(double logicalX) { return logicalX + ViewportCenterX; } private double LogicalToViewportY(double logicalY) { return -logicalY + ViewportCenterY; } // convert _viewport coordinates to logical coordinates private double ViewportToLogicalX(double viewportX) { return viewportX - ViewportCenterX; } private double ViewportToLogicalY(double viewportY) { return -viewportY + ViewportCenterY; } #endregion #region Logic Functions /// /// Evaluate the IK problem based on the current parameters and update the display /// shapes inside the current viewport. /// private void UpdateBonePositions() { // calculate the bone angles ValidSolution = IKSolver.CalcIK_2D_AllTwoBoneAnalytic( out m_solution1_bone1_angle, out m_solution1_bone2_angle, out m_solution2_bone1_angle, out m_solution2_bone2_angle, Bone1_Length, Bone2_Length, TargetPosX, TargetPosY ); // compute the the end points of the bones in logical space double solution1_cosAngle1 = Math.Cos(m_solution1_bone1_angle); double solution1_sinAngle1 = Math.Sin(m_solution1_bone1_angle); double solution1_cosAngle12 = Math.Cos(m_solution1_bone1_angle + m_solution1_bone2_angle); double solution1_sinAngle12 = Math.Sin(m_solution1_bone1_angle + m_solution1_bone2_angle); double solution2_cosAngle1 = Math.Cos(m_solution2_bone1_angle); double solution2_sinAngle1 = Math.Sin(m_solution2_bone1_angle); double solution2_cosAngle12 = Math.Cos(m_solution2_bone1_angle + m_solution2_bone2_angle); double solution2_sinAngle12 = Math.Sin(m_solution2_bone1_angle + m_solution2_bone2_angle); double solution1_bone1_endX = Bone1_Length * solution1_cosAngle1; double solution1_bone1_endY = Bone1_Length * solution1_sinAngle1; double solution1_bone2_endX = solution1_bone1_endX + Bone2_Length * solution1_cosAngle12; double solution1_bone2_endY = solution1_bone1_endY + Bone2_Length * solution1_sinAngle12; double solution2_bone1_endX = Bone1_Length * solution2_cosAngle1; double solution2_bone1_endY = Bone1_Length * solution2_sinAngle1; double solution2_bone2_endX = solution2_bone1_endX + Bone2_Length * solution2_cosAngle12; double solution2_bone2_endY = solution2_bone1_endY + Bone2_Length * solution2_sinAngle12; // output the coordinates in _viewport space _solution1_bone1_line.X1 = LogicalToViewportX( 0 ); _solution1_bone1_line.Y1 = LogicalToViewportY( 0 ); _solution1_bone1_line.X2 = LogicalToViewportX( solution1_bone1_endX ); _solution1_bone1_line.Y2 = LogicalToViewportY( solution1_bone1_endY ); _solution1_bone2_line.X1 = _solution1_bone1_line.X2; _solution1_bone2_line.Y1 = _solution1_bone1_line.Y2; _solution1_bone2_line.X2 = LogicalToViewportX( solution1_bone2_endX ); _solution1_bone2_line.Y2 = LogicalToViewportY( solution1_bone2_endY ); _solution2_bone1_line.X1 = LogicalToViewportX( 0 ); _solution2_bone1_line.Y1 = LogicalToViewportY( 0 ); _solution2_bone1_line.X2 = LogicalToViewportX( solution2_bone1_endX ); _solution2_bone1_line.Y2 = LogicalToViewportY( solution2_bone1_endY ); _solution2_bone2_line.X1 = _solution2_bone1_line.X2; _solution2_bone2_line.Y1 = _solution2_bone1_line.Y2; _solution2_bone2_line.X2 = LogicalToViewportX( solution2_bone2_endX ); _solution2_bone2_line.Y2 = LogicalToViewportY( solution2_bone2_endY ); // compute the range of our bones double minRange = Math.Abs(Bone1_Length - Bone2_Length); double maxRange = Bone1_Length + Bone2_Length; // draw the boundaries Canvas.SetLeft( _minRangeEllipse, LogicalToViewportX(-minRange) ); Canvas.SetTop( _minRangeEllipse, LogicalToViewportY(minRange) ); _minRangeEllipse.Width = 2*minRange; _minRangeEllipse.Height = 2*minRange; Canvas.SetLeft( _maxRangeEllipse, LogicalToViewportX(-maxRange) ); Canvas.SetTop( _maxRangeEllipse, LogicalToViewportY(maxRange) ); _maxRangeEllipse.Width = 2*maxRange; _maxRangeEllipse.Height = 2*maxRange; // draw the range of bone2 Canvas.SetLeft( _solution1_bone2RangeEllipse, LogicalToViewportX(solution1_bone1_endX - Bone2_Length) ); Canvas.SetTop( _solution1_bone2RangeEllipse, LogicalToViewportY(solution1_bone1_endY + Bone2_Length) ); _solution1_bone2RangeEllipse.Width = 2*Bone2_Length; _solution1_bone2RangeEllipse.Height = 2*Bone2_Length; Canvas.SetLeft( _solution2_bone2RangeEllipse, LogicalToViewportX(solution2_bone1_endX - Bone2_Length) ); Canvas.SetTop( _solution2_bone2RangeEllipse, LogicalToViewportY(solution2_bone1_endY + Bone2_Length) ); _solution2_bone2RangeEllipse.Width = 2*Bone2_Length; _solution2_bone2RangeEllipse.Height = 2*Bone2_Length; // draw the target Canvas.SetLeft( _targetEllipse, LogicalToViewportX(TargetPosX - _targetEllipse.Width/2) ); Canvas.SetTop( _targetEllipse, LogicalToViewportY(TargetPosY + _targetEllipse.Height/2) ); // draw the extended bone 1 line _solution1_bone1_extended_line.X1 = LogicalToViewportX( 0 ); _solution1_bone1_extended_line.Y1 = LogicalToViewportY( 0 ); _solution1_bone1_extended_line.X2 = LogicalToViewportX( (Bone1_Length + Bone2_Length) * solution1_cosAngle1 ); _solution1_bone1_extended_line.Y2 = LogicalToViewportY( (Bone1_Length + Bone2_Length) * solution1_sinAngle1 ); _solution2_bone1_extended_line.X1 = LogicalToViewportX( 0 ); _solution2_bone1_extended_line.Y1 = LogicalToViewportY( 0 ); _solution2_bone1_extended_line.X2 = LogicalToViewportX( (Bone1_Length + Bone2_Length) * solution2_cosAngle1 ); _solution2_bone1_extended_line.Y2 = LogicalToViewportY( (Bone1_Length + Bone2_Length) * solution2_sinAngle1 ); // draw the axes _xAxisLine.X1 = 0; _xAxisLine.Y1 = ViewportCenterY; _xAxisLine.X2 = ViewportWidth; _xAxisLine.Y2 = ViewportCenterY; _yAxisLine.X1 = ViewportCenterX; _yAxisLine.Y1 = 0; _yAxisLine.X2 = ViewportCenterX; _yAxisLine.Y2 = ViewportHieght; // draw the theta arcs { double theta1ArcRadius = 0.3 * Bone1_Length; PathGeometry theta1ArcPathGeo = _solution1_theta1ArcPath.Data as PathGeometry; PathFigure theta1ArcPathFigure = theta1ArcPathGeo.Figures[0]; ArcSegment theta1Arc = theta1ArcPathFigure.Segments[0] as ArcSegment; theta1Arc.Size = new Size(theta1ArcRadius,theta1ArcRadius); theta1ArcPathFigure.StartPoint = new Point( LogicalToViewportX(theta1ArcRadius), LogicalToViewportY(0) ); theta1Arc.Point = new Point( LogicalToViewportX( solution1_cosAngle1 * theta1ArcRadius ), LogicalToViewportY( solution1_sinAngle1 * theta1ArcRadius ) ); theta1Arc.SweepDirection = (m_solution1_bone1_angle >= 0) ? SweepDirection.Counterclockwise : SweepDirection.Clockwise; theta1Arc.IsLargeArc = (m_solution1_bone1_angle > Math.PI); } { double theta2ArcRadius = 0.3 * Math.Min( Bone1_Length, Bone2_Length ); PathGeometry theta2ArcPathGeo = _solution1_theta2ArcPath.Data as PathGeometry; PathFigure theta2ArcPathFigure = theta2ArcPathGeo.Figures[0]; ArcSegment theta2Arc = theta2ArcPathFigure.Segments[0] as ArcSegment; theta2Arc.Size = new Size(theta2ArcRadius,theta2ArcRadius); theta2ArcPathFigure.StartPoint = new Point( LogicalToViewportX( (Bone1_Length + theta2ArcRadius) * solution1_cosAngle1 ), LogicalToViewportY( (Bone1_Length + theta2ArcRadius) * solution1_sinAngle1 ) ); theta2Arc.Point = new Point( LogicalToViewportX( solution1_bone1_endX + solution1_cosAngle12 * theta2ArcRadius ), LogicalToViewportY( solution1_bone1_endY + solution1_sinAngle12 * theta2ArcRadius ) ); theta2Arc.SweepDirection = (m_solution1_bone2_angle >= 0) ? SweepDirection.Counterclockwise : SweepDirection.Clockwise; theta2Arc.IsLargeArc = false; } { double theta1ArcRadius = 0.3 * Bone1_Length; PathGeometry theta1ArcPathGeo = _solution2_theta1ArcPath.Data as PathGeometry; PathFigure theta1ArcPathFigure = theta1ArcPathGeo.Figures[0]; ArcSegment theta1Arc = theta1ArcPathFigure.Segments[0] as ArcSegment; theta1Arc.Size = new Size(theta1ArcRadius,theta1ArcRadius); theta1ArcPathFigure.StartPoint = new Point( LogicalToViewportX(theta1ArcRadius), LogicalToViewportY(0) ); theta1Arc.Point = new Point( LogicalToViewportX( solution2_cosAngle1 * theta1ArcRadius ), LogicalToViewportY( solution2_sinAngle1 * theta1ArcRadius ) ); theta1Arc.SweepDirection = (m_solution2_bone1_angle >= 0) ? SweepDirection.Counterclockwise : SweepDirection.Clockwise; theta1Arc.IsLargeArc = (m_solution2_bone1_angle > Math.PI); } { double theta2ArcRadius = 0.3 * Math.Min( Bone1_Length, Bone2_Length ); PathGeometry theta2ArcPathGeo = _solution2_theta2ArcPath.Data as PathGeometry; PathFigure theta2ArcPathFigure = theta2ArcPathGeo.Figures[0]; ArcSegment theta2Arc = theta2ArcPathFigure.Segments[0] as ArcSegment; theta2Arc.Size = new Size(theta2ArcRadius,theta2ArcRadius); theta2ArcPathFigure.StartPoint = new Point( LogicalToViewportX( (Bone1_Length + theta2ArcRadius) * solution2_cosAngle1 ), LogicalToViewportY( (Bone1_Length + theta2ArcRadius) * solution2_sinAngle1 ) ); theta2Arc.Point = new Point( LogicalToViewportX( solution2_bone1_endX + solution2_cosAngle12 * theta2ArcRadius ), LogicalToViewportY( solution2_bone1_endY + solution2_sinAngle12 * theta2ArcRadius ) ); theta2Arc.SweepDirection = (m_solution2_bone2_angle >= 0) ? SweepDirection.Counterclockwise : SweepDirection.Clockwise; theta2Arc.IsLargeArc = false; } // notify the user interface of changes NotifyPropertyChanged("Solution1_Bone1_Degrees"); NotifyPropertyChanged("Solution1_Bone2_Degrees"); NotifyPropertyChanged("Solution2_Bone1_Degrees"); NotifyPropertyChanged("Solution2_Bone2_Degrees"); } #endregion #region Event Handlers private void viewport_MouseLeftButtonDown(object sender, MouseButtonEventArgs e) { // capture the mouse to keep grabing MouseMove events if the user drags the // mouse outside of the _viewport bounds if (!_viewport.IsMouseCaptured) { _viewport.CaptureMouse(); } // update the target position Point viewportPos = e.GetPosition(_viewport); TargetPosX = ViewportToLogicalX( viewportPos.X ); TargetPosY = ViewportToLogicalY( viewportPos.Y ); } private void viewport_MouseLeftButtonUp(object sender, MouseButtonEventArgs e) { // release the captured mouse if (_viewport.IsMouseCaptured) { _viewport.ReleaseMouseCapture(); } } private void viewport_MouseMove(object sender, MouseEventArgs e) { // update the target position if we are still in a captured state // (i.e. the user has not released the mouse button) if (_viewport.IsMouseCaptured) { Point viewportPos = e.GetPosition(_viewport); TargetPosX = ViewportToLogicalX( viewportPos.X ); TargetPosY = ViewportToLogicalY( viewportPos.Y ); } } private void _thisWindow_SizeChanged(object sender, SizeChangedEventArgs e) { // update the display shapes based on the new window size UpdateBonePositions(); } private void _websiteLink_Click(object sender, RoutedEventArgs e) { System.Diagnostics.Process.Start( "http://www.ryanjuckett.com" ); } #endregion } }