CCD attempt

CGII/src/AtomicTransform.cpp

@@ -92,13 +92,48 @@ T clamp(T &v, T &c1, T &c2)
 	return v;
 }
 
+float angle(const Vec3 &v1, const Vec3 &v2)
+{
+	float d = dot(v1, v2);
+
+	float v1_length = length(v1);
+	float v2_length = length(v2);
+
+	return std::acos(d / (v1_length * v2_length));
+}
+
+/*
+fn angle(v1
+		 : cgmath::Vector2<f32>)
+	->f32
+{
+	let dot = v1.x * DEFAULT_DIRECTION.x + v1.y * DEFAULT_DIRECTION.y;
+
+	let v1_len = ((v1.x * v1.x) + (v1.y * v1.y)).sqrt();
+	let v2_len = ((DEFAULT_DIRECTION.x * DEFAULT_DIRECTION.x) + (DEFAULT_DIRECTION.y * DEFAULT_DIRECTION.y))
+					 .sqrt();
+
+	let angle = (dot / (v1_len * v2_len)).acos();
+
+	if
+		v1.x < 0.0
+		{
+			-angle
+		}
+	else
+	{
+		angle
+	}
+}
+*/
+
 void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
 {
 	/*Task: Implement parameter optimization*/
 
 	// optimize this that: target = this->calculate_matrix() * local_vector;
 
-	double result = 0.0;
+	double result = angle(local_vector, target);
 
 	if (inverse)
 		result = -result;