Uni/CGII
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Attempt to calculate positive/negative angle

Browse source
This commit is contained in:
hodasemi 2018-06-21 17:04:15 +02:00
parent 2b8aa08367
commit d1c4a03f5e
3 changed files with 30 additions and 117 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

108
CGII/src/AtomicTransform.cpp
View file

@ -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);
float v1_length = length(v1);
float v2_length = length(v2);
return std::acos(d / (v1_length * v2_length));
return std::acos(dot(v1, v2) / (length(v1) * length(v2))) * 180.0f / PI;
}
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)
{
return -z * rsin + y * x * (1 - rcos);
}
float plane_angle = angle(second_cross, 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;
if (plane_angle >= 90.0f)
{
return -angle(v1, v2);
}
else
{
return angle(v1, v2);
}
}
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);
}

37
CGII/src/AtomicTransform.h
View file

@ -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:
virtual float least_squares(const Vec3 &local_vector, const Vec3 &target, float r);
protected:
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;
};

2
CGII/src/IKViewer.cpp
View file

@ -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