/******************************************************************************
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
}
}