First commit

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

@@ -0,0 +1,134 @@
+#include "InverseKinemetics.h"
+
+#pragma region Core
+void InverseKinemetics::RL3 (Data *DataLine)
+{
+	float x = DataLine->X;
+	float z = DataLine->Z;
+
+	if ((x == 0) && (z == 0)) 
+		DataLine->l3 = 0;
+	else DataLine->l3 = hypot(z, x);
+
+	if (x < 0)
+		DataLine->l3 = DataLine->l3 * -1;
+}
+
+float InverseKinemetics::O1 (Data *DataLine)
+{
+	RL3(DataLine);
+
+	float x = DataLine->X;
+	float z = DataLine->Z;
+	float r;
+
+	if ((x != 0) & (z != 0)){
+		double a = (z / x);
+		return atan(a);
+	}
+	else
+		return atan(0.0); // Math Error Protection.
+}
+
+float InverseKinemetics::T1 (Data *DataLine)
+{
+	float x = DataLine->X;
+	float y = DataLine->Y;
+	float z = DataLine->Z;
+	float x2 = x * x;
+	float y2 = y * y;
+	float z2 = z * z;
+	float l32 = DataLine->l3 * DataLine->l3;
+
+	DataLine->s = sqrt(x2 + y2 + z2);
+
+	float t1 = 0;
+
+	float s2 = DataLine->s * DataLine->s;
+
+	if ((y == 0) && (DataLine->l3 == 0))
+		t1 = 0; // Math Error Protection.
+	else
+		t1 = (atan2(float(DataLine->l3), float(y)));
+
+	float a = (l1 * l1) - (l2*l2) + s2;
+	float b = 2 * l1 * DataLine->s;
+	float t2 = acos(a / b);
+
+	float ret = 0;
+
+	ret = t1 + abs(t2);
+
+	return ret;
+}
+
+float InverseKinemetics::T3 (Data *DataLine)
+{
+	return (acos((float)((l1*l1) + (l2*l2) - (DataLine->s*DataLine->s)) / (2 * l1 * l2)));
+}
+#pragma endregion Core
+
+#define PI 3.14159265
+
+void InverseKinemetics::setleg (bool Side, Data *DataLine)
+{
+	error = false;
+
+	if (Side == 0)
+	{
+		float a = ((float)90 - degrees(O1(DataLine)));
+		float b = ((float)180 - degrees(T1(DataLine)));
+		float c = degrees(T3(DataLine));
+
+		if ((a >= 0) & (a <= 180)
+			& (b >= 0) & (b <= 180)
+			& (c >= 0) & (c <= 180))
+		{
+			DataLine->O1 = a;
+			DataLine->O2 = b;
+			DataLine->O3 = c;
+
+			float d = sin( (float)b*(PI/180) );
+			if( d != 0 )
+				DataLine->X2 = d * l1;
+			else
+				DataLine->X2 = 0;
+
+			d = cos( (float)b*(PI/180) );
+			if( d != 0 )
+				DataLine->Y2 = d * l1;
+			else
+				DataLine->Y2 = 0;
+
+			d = sin( (float)(a-90)*(PI/180) );
+			if( d != 0 )
+				DataLine->Z2 = d * abs( DataLine->X2 );
+			else
+				DataLine->Z2 = 0;
+		}
+		else
+		{
+			error = true;
+		}
+	}
+	//else
+	//{
+	//	float a = ((float)90 + degrees(O1(DataLine)));
+	//	float b = degrees(T1(DataLine));
+	//	float c = ((float)180 - degrees(T3(DataLine)));
+	//
+	//	/*if ((a >= 0) & (a <= 180)
+	//	& (b >= 0) & (b <= 180)
+	//	& (c >= 0) & (c <= 180))
+	//	{*/
+	//	DataLine->O1 = a;
+	//	DataLine->O2 = b;
+	//	DataLine->O3 = c;
+	//	/*}
+	//	else
+	//	{
+	//	error = true;
+	//	}*/
+	//}
+}
+