Least squares function

CGII/src/AtomicTransform.cpp

@@ -102,30 +102,72 @@ float angle(const Vec3 &v1, const Vec3 &v2)
 	return std::acos(d / (v1_length * v2_length));
 }
 
-/*
-fn angle(v1
-		 : cgmath::Vector2<f32>)
-	->f32
+float a(float rcos, float x)
 {
-	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
-	}
+	return rcos + x * x * (1 - rcos);
+}
+
+float b(float rcos, float rsin, float x, float y, float z)
+{
+	return -z * rsin + y * x * (1 - rcos);
+}
+
+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 angler = r * PI / 180.0f;
+
+	float rcos = cos(angler);
+	float rsin = sin(angler);
+
+	float x = axis.x();
+	float y = axis.y();
+	float z = axis.z();
+
+	float xs = a(rcos, x) * local_vector.x() + b(rcos, rsin, x, y, z) * local_vector.y() + c(rcos, rsin, x, y, z) * local_vector.z();
+	float ys = d(rcos, rsin, x, y, z) * local_vector.x() + e(rcos, y) * local_vector.y() + f(rcos, rsin, x, y, z) * local_vector.z();
+	float zs = g(rcos, rsin, x, y, z) * local_vector.x() + h(rcos, rsin, x, y, z) * local_vector.y() + j(rcos, z);
+
+	float x_diff = xs - target.x();
+	float y_diff = ys - target.y();
+	float z_diff = zs - target.z();
+
+	float squares = x_diff * x_diff + y_diff * y_diff + z_diff * z_diff;
 }
-*/
 
 void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
 {
@@ -133,7 +175,9 @@ void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec
 
 	// optimize this that: target = this->calculate_matrix() * local_vector;
 
-	double result = angle(local_vector, target);
+	float first_guess = angle(local_vector, target);
+
+	double result = least_squares(local_vector, target, first_guess);
 
 	if (inverse)
 		result = -result;

CGII/src/AtomicTransform.h

@@ -14,30 +14,30 @@
 
 class Transform
 {
-public:
+  public:
 	//Calculates a matrix that represents the current transform.
 	virtual Mat4 calculate_matrix() = 0;
 
 	//Optimizes the current value, such that T * local_vector = target in a least-squares sense.
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target) = 0;
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target) = 0;
 };
 
 class StaticTransform : public Transform
 {
-public:
-	StaticTransform(const Mat4& t) : t(t) {}
+  public:
+	StaticTransform(const Mat4 &t) : t(t) {}
 
 	Mat4 calculate_matrix() { return t; }
-	void optimize_value(const Vec3& local_vector, const Vec3& target) { }
+	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);
 
@@ -45,17 +45,17 @@ public:
 	const double get_upper_limit() const;
 
 	//Sets the current scalar parameter. Ignore ud.
-	virtual void set_value(const double& value, void* ud = nullptr);
+	virtual void set_value(const double &value, void *ud = nullptr);
 
 	//Gets the current scalar parameter. Ignore ud.
-	const double get_value(void* ud = nullptr) const;
+	const double get_value(void *ud = nullptr) const;
 
 	//Calculates a matrix that represents the current transform.
 	virtual Mat4 calculate_matrix() = 0;
 
 	//Optimizes the current value, such that T * local_vector = target in a least-squares sense.
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target, bool inverse = false) = 0;
-	virtual void optimize_value(const Vec3& local_vector, const Vec3& target) { optimize_value(local_vector, target, false); }
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false) = 0;
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target) { optimize_value(local_vector, target, false); }
 
 	//Draws an indicator that visualizes the transform, including its limits.
 	virtual void drawIndicator(float size) = 0;
@@ -73,7 +73,7 @@ public:
 	//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,79 +82,82 @@ protected:
 
 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);
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false);
 
 	virtual void drawIndicator(float size);
 	virtual void drawActualIndicator(float size);
 
-protected:
+  private:
+	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);
+	virtual void optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse = false);
 
-	virtual void drawIndicator(float size) { };
-	virtual void drawActualIndicator(float size) { };
+	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:
-	InverseTransform(std::shared_ptr<AtomicTransform> t) : t(t) { }
+  public:
+	InverseTransform(std::shared_ptr<AtomicTransform> t) : t(t) {}
 
 	Mat4 calculate_matrix() { return cgv::math::inv(t->calculate_matrix()); }
-	void optimize_value(const Vec3& local_vector, const Vec3& target)
+	void optimize_value(const Vec3 &local_vector, const Vec3 &target)
 	{
 		t->optimize_value(local_vector, target, true);
 	}
 
-private:
+  private:
 	std::shared_ptr<AtomicTransform> t;
 };