I solved it barely knowing what Inverse Kinematics was. It should be fairly easy to figure out if you know some trigonometry. Of course, it depends on the complexity of the shape. What i was solving was a 3DOF leg for a legged robot.
This is my solution in Java (hope you can make out how it works):
import java.lang.Math;
public class Leg {
public double TipX, TipY, TipZ; //3D coordinate for where the legs tip will be calculated to be.
public double KneeX, KneeY, KneeZ, CoxaX, CoxaZ; //Coordinates for joints in the leg.
public double Coxa, Femur, Tibia; //Angles for the joints in radians
public double Distance, Distance2D; //Variables needed in the calculation, Distance2D is the distance from the legs attachment point to the tip projected to a XZ plane
public boolean Calculate() {
Distance2D = Math.sqrt( Math.pow( TipX, 2 ) + Math.pow( TipZ, 2 ) );
if (TipX < 0.0) Distance2D = -Distance2D;
Distance2D -= 50.0;
if ((Distance2D > 150.0)) {
return false;
}
if ((Distance2D < -150.0)) {
return false;
}
if (Distance2D != 0.0) {
Coxa = Math.asin( TipZ / (Distance2D + 50.0) );
}
else {
Coxa = 0.0;
}
Distance = Math.sqrt( Math.pow( Distance2D, 2 ) + Math.pow( TipY, 2 ) );
if ((Distance > 150.0) | (Distance < 50.0)) {
return false;
}
if (Distance2D >= 0.0) {
Femur = (Math.acos(TipY / Distance) + Math.acos((Math.pow(50,2)+Math.pow(Distance,2)-Math.pow(100,2))/(2*50*Distance)));
}
else {
Femur = (-Math.acos(TipY / Distance) + Math.acos((Math.pow(50,2)+Math.pow(Distance,2)-Math.pow(100,2))/(2*50*Distance)));
}
Tibia = Math.acos((Math.pow(50,2)+Math.pow(100,2)-Math.pow(Distance,2))/(2*50*100));
KneeX = Math.cos(Femur - (Math.PI / 2)) * 50.0 + 50.0;
KneeY = -Math.sin(Femur - (Math.PI / 2)) * 50.0;
KneeZ = Math.cos(Coxa - (Math.PI / 2)) * KneeX;
KneeX = -Math.sin(Coxa - (Math.PI / 2)) * KneeX;
CoxaZ = Math.cos(Coxa - (Math.PI / 2)) * 50.0;
CoxaX = -Math.sin(Coxa - (Math.PI / 2)) * 50.0;
return true;
}
public void SetPosition(int x, int y, int z) {
TipX = (double)x;
TipY = (double)y;
TipZ = (double)z;
}
public void SetPosition(double x, double y, double z) {
TipX = x;
TipY = y;
TipZ = z;
}
public void IncPosition(int x, int y, int z) {
TipX += (double)x;
TipY += (double)y;
TipZ += (double)z;
}
public void IncPosition(double x, double y, double z) {
TipX += x;
TipY += y;
TipZ += z;
}
public boolean Calculate( int x, int y, int z ) {
TipX = x;
TipY = y;
TipZ = z;
return Calculate();
}
public boolean Calculate( double x, double y, double z ) {
TipX = x;
TipY = y;
TipZ = z;
return Calculate();
}
}
The dimensions of the leg is following:
Length of coxa (from attachment point for the leg to the next joint): 30 mm
Length of femur: 50 mm
Length of tibia: 100 mm