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 b(float rcos, float rsin, float x, float y, float z)
+	float plane_angle = angle(second_cross, v2);
-{
-	return -z * rsin + y * x * (1 - rcos);
-}
 
-float c(float rcos, float rsin, float x, float y, float z)
+	if (plane_angle >= 90.0f)
-{
+	{
-	return y * rsin + z * x * (1 - rcos);
+		return -angle(v1, v2);
-}
+	}
-
+	else
-float d(float rcos, float rsin, float x, float y, float z)
+	{
-{
+		return angle(v1, v2);
-	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

@@ -14,7 +14,7 @@
 
 class Transform
 {
-  public:
+public:
 	//Calculates a matrix that represents the current transform.
 	virtual Mat4 calculate_matrix() = 0;
 
@@ -24,20 +24,20 @@ class Transform
 
 class StaticTransform : public Transform
 {
-  public:
+public:
 	StaticTransform(const Mat4 &t) : t(t) {}
 
 	Mat4 calculate_matrix() { return t; }
 	void optimize_value(const Vec3 &local_vector, const Vec3 &target) {}
 
-  private:
+private:
 	Mat4 t;
 };
 
 //Represents an arbitrary affine transform with exactly one scalar parameter
 class AtomicTransform : public cgv::gui::control_provider<double>, public Transform
 {
-  public:
+public:
 	//Sets the limits of the scalar parameter
 	void set_limits(double lower, double upper);
 
@@ -73,7 +73,7 @@ class AtomicTransform : public cgv::gui::control_provider<double>, public Transf
 	//Get the order in which the transform is specified in the animation file.
 	int get_index_in_amc() const { return index_in_amc; }
 
-  protected:
+protected:
 	double lower_limit, upper_limit;
 	double value;
 	std::string title;
@@ -82,7 +82,7 @@ class AtomicTransform : public cgv::gui::control_provider<double>, public Transf
 
 class AtomicRotationTransform : public AtomicTransform
 {
-  public:
+public:
 	AtomicRotationTransform(Vec3 axis);
 	virtual Mat4 calculate_matrix();
 	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false);
@@ -90,34 +90,31 @@ class AtomicRotationTransform : public AtomicTransform
 	virtual void drawIndicator(float size);
 	virtual void drawActualIndicator(float size);
 
-  private:
+protected:
-	virtual float least_squares(const Vec3 &local_vector, const Vec3 &target, float r);
-
-  protected:
 	Vec3 axis;
 };
 
 class AtomicXRotationTransform : public AtomicRotationTransform
 {
-  public:
+public:
 	AtomicXRotationTransform() : AtomicRotationTransform(Vec3(1, 0, 0)) { title = "X-Rotation"; }
 };
 
 class AtomicYRotationTransform : public AtomicRotationTransform
 {
-  public:
+public:
 	AtomicYRotationTransform() : AtomicRotationTransform(Vec3(0, 1, 0)) { title = "Y-Rotation"; }
 };
 
 class AtomicZRotationTransform : public AtomicRotationTransform
 {
-  public:
+public:
 	AtomicZRotationTransform() : AtomicRotationTransform(Vec3(0, 0, 1)) { title = "Z-Rotation"; }
 };
 
 class AtomicTranslationTransform : public AtomicTransform
 {
-  public:
+public:
 	AtomicTranslationTransform(int dim);
 	virtual Mat4 calculate_matrix();
 	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false);
@@ -125,31 +122,31 @@ class AtomicTranslationTransform : public AtomicTransform
 	virtual void drawIndicator(float size){};
 	virtual void drawActualIndicator(float size){};
 
-  private:
+private:
 	int dim;
 };
 
 class AtomicXTranslationTransform : public AtomicTranslationTransform
 {
-  public:
+public:
 	AtomicXTranslationTransform() : AtomicTranslationTransform(0) { title = "X-Translation"; }
 };
 
 class AtomicYTranslationTransform : public AtomicTranslationTransform
 {
-  public:
+public:
 	AtomicYTranslationTransform() : AtomicTranslationTransform(1) { title = "Y-Translation"; }
 };
 
 class AtomicZTranslationTransform : public AtomicTranslationTransform
 {
-  public:
+public:
 	AtomicZTranslationTransform() : AtomicTranslationTransform(2) { title = "Z-Translation"; }
 };
 
 class InverseTransform : public Transform
 {
-  public:
+public:
 	InverseTransform(std::shared_ptr<AtomicTransform> t) : t(t) {}
 
 	Mat4 calculate_matrix() { return cgv::math::inv(t->calculate_matrix()); }
@@ -158,6 +155,6 @@ class InverseTransform : public Transform
 		t->optimize_value(local_vector, target, true);
 	}
 
-  private:
+private:
 	std::shared_ptr<AtomicTransform> t;
 };

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