Robidouille - Robot Arm : Phase 2 - Let's kick it up a degree !

Vector diff = target - points[2]; Vector xAxis = points[2] - points[0]; xAxis.Normalize(); Vector yAxis = points[2] - points[1]; yAxis.Normalize(); Vector zAxis = xAxis ^ yAxis; zAxis.Normalize(); Matrix rotate; rotate.SetToRotation(zAxis, PI / 2); xAxis = rotate * xAxis; yAxis = rotate * yAxis; result.Z = zAxis * diff; Vector diffXY = diff - result.Z * zAxis; float cosXY = xAxis * yAxis; float cosXV = xAxis * diffXY; float cosYV = yAxis * diffXY; float sinXY = (xAxis ^ yAxis) * zAxis; float sinXV = (xAxis ^ diffXY) * zAxis; float sinYV = (yAxis ^ diffXY) * zAxis; result.X = cosXV - sinXV*(cosXY/sinXY); result.Y = cosYV + sinYV*(cosXY/sinXY);

void Vector::operator/=(float value)
{
	X /= value;
	Y /= value;
	Z /= value;
}

void Vector::operator*=(float value) {
	X *= value;
	Y *= value;
	Z *= value;
}

float Vector::GetNorm() {
	return sqrt(X*X + Y*Y + Z*Z);
}

void Vector::Normalize() {
	this /= GetNorm();
}

float operator*(Vector vector1, Vector vector2)
{
	return vector1.X*vector2.X + vector1.Y*vector2.Y + vector1.Z*vector2.Z;
}

Vector operator+(Vector vector1, Vector vector2) {
	Vector result;
	result.X = vector1.X + vector2.X;
	result.Y = vector1.Y + vector2.Y;
	result.Z = vector1.Z + vector2.Z;
	return result;
}

Vector operator-(Vector vector1, Vector vector2) {
	Vector result;
	result.X = vector1.X - vector2.X;
	result.Y = vector1.Y - vector2.Y;
	result.Z = vector1.Z - vector2.Z;
	return result;
}

Vector operator^(Vector vector1, Vector vector2) {
	Vector result;
	result.X = vector1.Y*vector2.Z - vector1.Z*vector2.Y;
	result.Y = vector1.Z*vector2.X - vector1.X*vector2.Z;
	result.Z = vector1.X*vector2.Y - vector1.Y*vector2.X;
	return result;
}

Vector operator*(Vector vector, float scalar) {
	Vector result;
	result.X = vector.X*scalar;
	result.Y = vector.Y*scalar;
	result.Z = vector.Z*scalar;
	return result;
}

void Matrix::SetToRotation(Vector axis, float angle) {
	float cc = cos(angle);
	float ss = sin(angle);

	float x2 = axis.X*axis.X;
	float y2 = axis.Y*axis.Y;
	float z2 = axis.Z*axis.Z;

	float xy = axis.X*axis.Y;
	float yz = axis.Y*axis.Z;
	float xz = axis.X*axis.Z;

	II = x2 + (1-x2) * cc          ; IJ = xy * (1-cc) - axis.Z * ss ; IK = xz * (1-cc) + axis.Y * ss;
	JI = xy * (1-cc) + axis.Z * ss ; JJ = y2 + (1-y2) * cc          ; JK = yz * (1-cc) - axis.X * ss;
	KI = xz * (1-cc) - axis.Y * ss ; KJ = yz * (1-cc) + axis.X * ss ; KK = z2 + (1-z2) * cc;
}

Vector operator*(Matrix matrix, Vector vector) {
	Vector result;
	result.X = matrix.II*vector.X + matrix.IJ*vector.Y + matrix.IK*vector.Z;
	result.Y = matrix.JI*vector.X + matrix.JJ*vector.Y + matrix.JK*vector.Z;
	result.Z = matrix.KI*vector.X + matrix.KJ*vector.Y + matrix.KK*vector.Z;
	return result;
}