Attempt to calculate positive/negative angle

CGII/src/AtomicTransform.cpp

@@ -94,94 +94,24 @@ T clamp(T &v, T &c1, T &c2)
 
 float angle(const Vec3 &v1, const Vec3 &v2)
 {
-	float d = dot(v1, v2);
+	return std::acos(dot(v1, v2) / (length(v1) * length(v2))) * 180.0f / PI;
-
-	float v1_length = length(v1);
-	float v2_length = length(v2);
-
-	return std::acos(d / (v1_length * v2_length));
 }
 
-float a(float rcos, float x)
+float optimize_angle(const Vec3 &v1, const Vec3 &v2)
 {
-	return rcos + x * x * (1 - rcos);
+	Vec3 first_cross = cross(v1, v2);
+	Vec3 second_cross = cross(v1, first_cross);
+
+	float plane_angle = angle(second_cross, v2);
+
+	if (plane_angle >= 90.0f)
+	{
+		return -angle(v1, v2);
 	}
-
+	else
-float b(float rcos, float rsin, float x, float y, float z)
 	{
-	return -z * rsin + y * x * (1 - rcos);
+		return angle(v1, v2);
 	}
-
-float c(float rcos, float rsin, float x, float y, float z)
-{
-	return y * rsin + z * x * (1 - rcos);
-}
-
-float d(float rcos, float rsin, float x, float y, float z)
-{
-	return z * rsin + x * y * (1 - rcos);
-}
-
-float e(float rcos, float y)
-{
-	return rcos + y * y * (1 - rcos);
-}
-
-float f(float rcos, float rsin, float x, float y, float z)
-{
-	return -x * rsin + z * y * (1 - rcos);
-}
-
-float g(float rcos, float rsin, float x, float y, float z)
-{
-	return -y * rsin + x * z * (1 - rcos);
-}
-
-float h(float rcos, float rsin, float x, float y, float z)
-{
-	return x * rsin + y * z * (1 - rcos);
-}
-
-float j(float rcos, float z)
-{
-	rcos + z *z *(1 - rcos);
-}
-
-float AtomicRotationTransform::least_squares(const Vec3 &local_vector, const Vec3 &target, float r)
-{
-	float pii = PI;
-	float hundertachtzig = 180.0f;
-
-	float angle = r * pii / hundertachtzig;
-
-	float rcos = cos(angle);
-	float rsin = sin(angle);
-
-	float x = axis.x();
-	float y = axis.y();
-	float z = axis.z();
-
-	float xt = target.x();
-	float yt = target.y();
-	float zt = target.z();
-
-	float xl = local_vector.x();
-	float yl = local_vector.y();
-	float zl = local_vector.z();
-
-	float xs = a(rcos, x) * xl + b(rcos, rsin, x, y, z) * yl + c(rcos, rsin, x, y, z) * zl;
-	float ys = d(rcos, rsin, x, y, z) * xl + e(rcos, y) * yl + f(rcos, rsin, x, y, z) * zl;
-	float zs = g(rcos, rsin, x, y, z) * xl + h(rcos, rsin, x, y, z) * yl + j(rcos, z) * zl;
-
-	float x_diff = xs - xt;
-	float y_diff = ys - yt;
-	float z_diff = zs - zt;
-
-	float squares = std::sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff);
-
-	float first_diff = 2 * (xl * ((pii * sin((pii * r) / hundertachtzig)) / hundertachtzig - (pii * x * x * sin((pii * r) / hundertachtzig)) / hundertachtzig) + yl * ((pii * z * cos((pii * r) / hundertachtzig)) / hundertachtzig - (pii * x * y * sin((pii * r) / hundertachtzig)) / hundertachtzig) - zl * ((pii * y * cos((pii * r) / hundertachtzig)) / hundertachtzig + (pii * x * z * sin((pii * r) / hundertachtzig)) / hundertachtzig)) * (xt - xl * ((1 - cos((pii * r) / hundertachtzig)) * x * x + cos((pii * r) / hundertachtzig)) + yl * (z * sin((pii * r) / hundertachtzig) + x * y * (cos((pii * r) / hundertachtzig) - 1)) - zl * (y * sin((pii * r) / hundertachtzig) - x * z * (cos((pii * r) / hundertachtzig) - 1))) + 2 * (zl * ((pii * sin((pii * r) / hundertachtzig)) / hundertachtzig - (pii * z * z * sin((pii * r) / hundertachtzig)) / hundertachtzig) + xl * ((pii * y * cos((pii * r) / hundertachtzig)) / hundertachtzig - (pii * x * z * sin((pii * r) / hundertachtzig)) / hundertachtzig) - yl * ((pii * x * cos((pii * r) / hundertachtzig)) / hundertachtzig + (pii * y * z * sin((pii * r) / hundertachtzig)) / hundertachtzig)) * (zt + xl * (y * sin((pii * r) / hundertachtzig) + x * z * (cos((pii * r) / hundertachtzig) - 1)) - yl * (x * sin((pii * r) / hundertachtzig) - y * z * (cos((pii * r) / hundertachtzig) - 1)) - zl * ((1 - cos((pii * r) / hundertachtzig)) * z * z + cos((pii * r) / hundertachtzig))) + 2 * (xl * ((pii * z * cos((pii * r) / hundertachtzig)) / hundertachtzig + (pii * x * y * sin((pii * r) / hundertachtzig)) / hundertachtzig) - yl * ((pii * sin((pii * r) / hundertachtzig)) / hundertachtzig - (pii * y * y * sin((pii * r) / hundertachtzig)) / hundertachtzig) + zl * ((pii * y * cos((pii * r) / hundertachtzig)) / hundertachtzig + (pii * x * z * sin((pii * r) / hundertachtzig)) / hundertachtzig)) * (yl * ((1 - cos((pii * r) / hundertachtzig)) * y * y + cos((pii * r) / hundertachtzig)) - yt + xl * (z * sin((pii * r) / hundertachtzig) - x * y * (cos((pii * r) / hundertachtzig) - 1)) + zl * (y * sin((pii * r) / hundertachtzig) - x * z * (cos((pii * r) / hundertachtzig) - 1)));
-
-	return squares;
 }
 
 void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
@@ -197,23 +127,11 @@ void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec
 
 	result = (double)angle(lp, tp);
 
-	result = result * 180 / PI;
-
-	//std::cout << "angle before bounds check: " << result << std::endl;
-
-
-	//float first_guess = angle(local_vector, target);
-
-	//double result = least_squares(local_vector, target, first_guess);
-
 	if (inverse)
 		result = -result;
 
 	result = clamp(result, lower_limit, upper_limit);
 
-
-	//std::cout << "angle after bounds check: " << result << std::endl;
-
 	set_value(result);
 }
 

CGII/src/AtomicTransform.h

@@ -90,9 +90,6 @@ class AtomicRotationTransform : public AtomicTransform
 	virtual void drawIndicator(float size);
 	virtual void drawActualIndicator(float size);
 
-  private:
-	virtual float least_squares(const Vec3 &local_vector, const Vec3 &target, float r);
-
 protected:
 	Vec3 axis;
 };

CGII/src/IKViewer.cpp

@@ -218,8 +218,6 @@ void IKViewer::optimize()
 	/*Task 3.3: Implement CCD	*/
 	for (int i = 0; i < max_iterations; i++)
 	{
-		std::cout << "iteration: " << i << std::endl;
-
 		int kc_size = kinematic_vector.size();
 
 		// reverse iterate through kinematic chain