First commit

2012-simulation/ik-visualiser-vs/Hexapod app/Leg.cpp

@@ -0,0 +1,187 @@
+//Leg.cpp
+
+#include "Leg.h"
+
+//Initial
+Leg::Leg()
+{
+	Coordinate = D3DXVECTOR3( 0, 0, 0 );
+	Error = 1;
+}
+
+Leg::~Leg()
+{
+
+}
+
+void Leg::GetHDC( HDC hdc )
+{
+	HDCBuffer = hdc;
+}
+
+void Leg::GetHDCSecondBuffer( HDC hdc )
+{
+	HDCSecondBuffer = hdc;
+}
+
+void Leg::GetPossibleMoveSpace(  )
+{
+	if( MoveSpaceRenderd != 1 )
+	{
+		InverseKinemetics IK;
+		Data *D = new Data;
+
+		MoveSpace.clear();
+
+		for (float y = -25; y < 26; y += 0.05)
+		{
+			for (float x = -25; x < 26; x += 0.05)
+			{
+				/*for (int z = -25; z < 26; z++)
+				{*/
+				D->Input( D3DXVECTOR3( x, y, Coordinate.z ) );
+				IK.setleg( 0, D );
+
+				if( IK.error == 0 )
+				{
+					MoveSpace.push_back( struct D3DXVECTOR3( x, y, 0 ));
+				}
+				/*}*/
+			}
+		}
+		MoveSpaceRenderd = 1;
+	}
+}
+
+//Overload
+D3DXVECTOR3 operator*(const D3DXVECTOR3& a, const D3DXVECTOR3& b)
+{
+   return D3DXVECTOR3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+
+//Input
+void Leg::Coordinates( float ix, float iy, float iz )
+{
+	Coordinate.x = ix;
+	Coordinate.y = iy;
+	Coordinate.z = iz;
+}
+
+void Leg::Coordinates( D3DXVECTOR3 i )
+{
+	Coordinate = i;
+}
+
+void Leg::Offset( float ix, float iy, float iz )
+{
+	OffsetVector = D3DXVECTOR3( ix, iy, iz );
+}
+
+void Leg::Offset( D3DXVECTOR3 i )
+{
+	OffsetVector = i;
+}
+
+void Leg::Size( float ix, float iy, float iz )
+{
+	SizeVector = D3DXVECTOR3( ix, iy, iz );
+}
+
+void Leg::Size( D3DXVECTOR3 i )
+{
+	SizeVector = i;
+}
+
+//Functions
+void Leg::CalculateLegRight(  )
+{
+	Data *D = new Data;
+	InverseKinemetics IK;
+
+	D->Input( Coordinate );
+
+	IK.setleg( 0, D );
+
+	Error = IK.error;
+
+	ModelSpaceHip  = D3DXVECTOR3( 0, 0, 0 );
+	ModelSpaceKnee = D3DXVECTOR3( D->X2, D->Y2*-1, D->Z2 );
+	ModelSpaceFeet = Coordinate;
+}
+
+void Leg::CalculateWorldSpaceLineModel(  )
+{
+	WorldSpaceLineModel.clear();
+	WorldSpaceLineModel.push_back( ModelSpaceHip + OffsetVector );
+	WorldSpaceLineModel.push_back( ( ModelSpaceKnee * SizeVector ) + OffsetVector );
+	WorldSpaceLineModel.push_back( ( ModelSpaceFeet * SizeVector ) + OffsetVector );
+}
+
+void Leg::PrintMoveSpaceOnWorldSpace(  )
+{
+	if( MoveSpaceRenderd )
+	{
+		HPEN pen, oldPen;
+
+		pen = CreatePen(PS_SOLID, 2, RGB(0, 32, 20));    
+		oldPen = (HPEN)SelectObject(HDCSecondBuffer, pen);                
+
+		MoveToEx( HDCSecondBuffer, OffsetVector.x, OffsetVector.y, 0 );
+
+		for (int i=0; i<MoveSpace.size(); i++)
+		{
+			int X = OffsetVector.x+(MoveSpace[i].x*SizeVector.x);
+			int Y = OffsetVector.y+(MoveSpace[i].y*SizeVector.y);
+			SetPixel( HDCSecondBuffer, X, Y, RGB(0, 32, 20) );
+		}
+
+		SelectObject(HDCSecondBuffer, oldPen); 
+		DeleteObject(pen);
+	}
+}
+
+void Leg::PrintWorldSpaceLineModel(  )
+{
+	HPEN pen, oldPen;
+
+	int Size = 15;
+
+	pen = CreatePen(PS_SOLID, 2, RGB(150, 150, 150));    
+	oldPen = (HPEN)SelectObject(HDCBuffer, pen);                
+
+	MoveToEx( HDCBuffer, WorldSpaceLineModel[0].x, 
+		WorldSpaceLineModel[0].y, 0 );
+
+	LineTo( HDCBuffer, WorldSpaceLineModel[1].x,
+		WorldSpaceLineModel[1].y);
+
+	LineTo( HDCBuffer, WorldSpaceLineModel[2].x,
+		WorldSpaceLineModel[2].y );
+
+	SelectObject(HDCBuffer, oldPen); 
+	DeleteObject(pen);
+}
+
+void Leg::Print3DLinesOnWorldSpace(  )
+{
+	HPEN pen, oldPen;
+
+	pen = CreatePen(PS_SOLID, 2, RGB(255, 0, 0));    
+	oldPen = (HPEN)SelectObject(HDCBuffer, pen);   
+
+	MoveToEx( HDCBuffer, OffsetVector.x, 0, 0 );
+	LineTo( HDCBuffer, OffsetVector.x, 800 );
+
+	SelectObject(HDCBuffer, oldPen); 
+	DeleteObject(pen);
+
+	pen = CreatePen(PS_SOLID, 2, RGB(0, 0, 255));    
+	oldPen = (HPEN)SelectObject(HDCBuffer, pen);   
+
+	MoveToEx( HDCBuffer, 0, OffsetVector.y, 0 );
+	LineTo( HDCBuffer, 800, OffsetVector.y );
+
+	SelectObject(HDCBuffer, oldPen); 
+	DeleteObject(pen);
+}
+