commit c4634a135d9778d5457718a4b93bf253b7ad8963
parent 297250ada07784787724b90a135c349752497744
Author: Christophe Coustet <christophe.coustet@meso-star.com>
Date: Tue, 18 Oct 2016 17:12:06 +0200
BugFix: 1 axis pivot pointing computations
The computations were not really in the YZ plane.
Adapt pivot testing to catch this.
Diffstat:
2 files changed, 69 insertions(+), 40 deletions(-)
diff --git a/src/sanim_node.c b/src/sanim_node.c
@@ -72,7 +72,11 @@ set_pivot_transform(const double angles[3], double transform[12]) {
}
static double*
-set_node_transform(struct sanim_node* node, const char include_pivot, double transform[12]) {
+set_node_transform
+ (struct sanim_node* node,
+ const char include_pivot,
+ double transform[12])
+{
double tmp[12];
ASSERT(node && node->data && transform);
if (include_pivot && node->data->pivot_data) {
@@ -103,7 +107,10 @@ compose_node_transform(struct sanim_node* node, double transform[12]) {
}
static void
-node_get_transform(struct sanim_node* node, const char include_own_pivot, double transform[12])
+node_get_transform
+ (struct sanim_node* node,
+ const char include_own_pivot,
+ double transform[12])
{
struct sanim_node* ptr;
ASSERT(node && node->data && transform);
@@ -115,26 +122,21 @@ node_get_transform(struct sanim_node* node, const char include_own_pivot, double
}
}
-static res_T
+static void
compute_single_axis_angle
-(const double ref_normal[3],
- const double rotated_n[3],
- double* angle )
+ (const double ref_normal[3],
+ const double rotated_n[3],
+ double* angle )
{
ASSERT(ref_normal && rotated_n && angle);
ASSERT(d3_is_normalized(rotated_n));
ASSERT(d3_is_normalized(ref_normal));
- ASSERT(ref_normal[0] == 0); /* ref_normal in the YZ plane */
- if (fabs(rotated_n[0]) > 0.9) {
- /* cannot be obtained by X-rotate */
- return RES_BAD_ARG;
- }
- /* solve in the YZ plane */
+ /* normals should be in the YZ plane */
+ ASSERT(ref_normal[0] == 0);
+ ASSERT(rotated_n[0] == 0);
*angle =
atan2(-(ref_normal[2] * rotated_n[1] - ref_normal[1] * rotated_n[2]),
ref_normal[1] * rotated_n[1] + ref_normal[2] * rotated_n[2]);
-
- return RES_OK;
}
static res_T
@@ -161,11 +163,18 @@ pivot_solve_single_axis_sun
d33_transpose(inv, mat); /* no scale factors: inverse is transpose */
d33_muld3(local_in, inv, in_dir);
+ /* solve in the YZ plane */
+ local_in[0] = 0;
+ if (d3_normalize(local_in, local_in) < 0.25) {
+ /* not really in the YZ-plane */
+ return RES_BAD_ARG;
+ }
+
/* rotated_n = -local_in */
d3_muld(rotated_n, local_in, -1);
- ASSERT(d3_is_normalized(rotated_n));
- return compute_single_axis_angle(ref_normal, rotated_n, angle);
+ compute_single_axis_angle(ref_normal, rotated_n, angle);
+ return RES_OK;
}
FINLINE res_T
@@ -173,7 +182,6 @@ pivot_solve_single_axis_line
(struct sanim_node* node,
const double in_dir[3])
{
- res_T res = RES_OK;
double mat[12], inv[9];
double local_in[3], rotated_n[3], local_out[3], local_target[3], ref_point[3];
const double* ref_normal;
@@ -209,19 +217,29 @@ pivot_solve_single_axis_line
double pivot[12];
/* compute rotated_n */
d3_sub(local_out, local_target, ref_point);
- if (0 == d3_normalize(local_out, local_out)) {
- /* cannot target so close to the device */
+
+ /* solve in the YZ plane */
+ local_in[0] = 0;
+ if (d3_normalize(local_in, local_in) < 0.25) {
+ /* not really in the YZ-plane */
return RES_BAD_ARG;
}
- /* compute rotated_n = bisectrix of local_in and out_dir */
+ local_out[0] = 0;
+ if (d3_normalize(local_out, local_out) < 0.25) {
+ /* not really in the YZ-plane */
+ return RES_BAD_ARG;
+ }
+
+ /* rotated_n = bisectrix of local_in and out_dir */
d3_sub(rotated_n, local_out, local_in);
- if (0 == d3_normalize(rotated_n, rotated_n)) {
- /* cannot be obtained by X-rotate */
+ if (d3_normalize(rotated_n, rotated_n) < 1e-4) {
+ /* tangent rays */
return RES_BAD_ARG;
}
+ ASSERT(rotated_n[0] == 0);
+
previous_angle = angles[0];
- res = compute_single_axis_angle(ref_normal, rotated_n, angles); /* X-rotate */
- if (res != RES_OK) return res;
+ compute_single_axis_angle(ref_normal, rotated_n, angles); /* X-rotate */
delta = fabs(previous_angle - angles[0]);
if (delta < 1e-10 || ++cpt == 25) break;
set_pivot_transform(angles, pivot);
@@ -259,17 +277,30 @@ pivot_solve_single_axis_dir
d33_transpose(inv, mat); /* no scale factors: inverse is transpose */
d33_muld3(local_in, inv, in_dir);
d33_muld3(local_out, inv, node->data->pivot_data->tracking.data.out_dir.u);
- ASSERT(d3_is_normalized(local_out));
- /* compute rotated_n = bisectrix of local_in and out_dir */
+ /* solve in the YZ plane */
+ local_in[0] = 0;
+ if (d3_normalize(local_in, local_in) < 0.25) {
+ /* not really in the YZ-plane */
+ return RES_BAD_ARG;
+ }
+ local_out[0] = 0;
+ if (d3_normalize(local_out, local_out) < 0.25) {
+ /* not really in the YZ-plane */
+ return RES_BAD_ARG;
+ }
+
+ /* rotated_n = bisectrix of local_in and out_dir */
d3_sub(rotated_n, local_out, local_in);
- if (0 == d3_normalize(rotated_n, rotated_n)) {
- /* cannot be obtained by X-rotate */
+ if (d3_normalize(rotated_n, rotated_n) < 1e-4) {
+ /* tangent rays */
return RES_BAD_ARG;
}
+ ASSERT(rotated_n[0] == 0);
angle = &node->data->pivot_data->pivot_angles[0]; /* X-rotate */
- return compute_single_axis_angle(ref_normal, rotated_n, angle);
+ compute_single_axis_angle(ref_normal, rotated_n, angle);
+ return RES_OK;
}
FINLINE res_T
diff --git a/src/test_sanim_node_pivot.c b/src/test_sanim_node_pivot.c
@@ -68,12 +68,11 @@ main(int argc, char** argv)
/* rotation axis is Y after positioning: cannot accomodate in_dir */
CHECK(sanim_node_solve_pivot(&t2.node, in_dir), RES_BAD_ARG);
- d3(in_dir, 1, 0, -1);
+ d3(in_dir, 1, 0.2, -1);
CHECK(sanim_node_solve_pivot(&t2.node, in_dir), RES_OK);
CHECK(my_type_get_transform(&t3, transform), RES_OK);
- d3(n, 0, 0, 1);
- d33_muld3(n, transform, n);
- CHECK(eq_eps(d3_dot(in_dir, n), -d3_normalize(tmp, in_dir), 1e-10), 1);
+ d33_muld3(n, transform, pivot1.data.pivot1.ref_normal);
+ CHECK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-10), 1);
CHECK(d3_eq_eps(transform + 9, d3(tmp, -sqrt(2), 3, 2), 1e-10), 1);
CHECK(my_type_release(&t1), RES_OK);
@@ -124,13 +123,12 @@ main(int argc, char** argv)
CHECK(my_type_set_translation(&t2, transl), RES_OK);
CHECK(my_type_set_translation(&t3, transl), RES_OK);
- d3(in_dir, 1, 0, -1);
+ d3(in_dir, 1, -0.3, -1);
CHECK(sanim_node_solve_pivot(&t2.node, in_dir), RES_OK);
CHECK(my_type_get_transform(&t3, transform), RES_OK);
- d3(n, 0, 0, 1);
- d33_muld3(tmp, transform, n);
- CHECK(d3_eq_eps(n, tmp, 1e-10), 1);
- CHECK(d3_eq_eps(transform + 9, d3(tmp, -1, 3, 3), 1e-10), 1);
+ d33_muld3(n, transform, pivot1.data.pivot1.ref_normal);
+ CHECK(d3_eq_eps(pivot1.data.pivot1.ref_normal, n, 1e-10), 1);
+ CHECK(d3_eq_eps(transform + 9, d3(n, -1, 3, 3), 1e-10), 1);
CHECK(my_type_release(&t1), RES_OK);
CHECK(my_type_release(&t2), RES_OK);
@@ -157,7 +155,7 @@ main(int argc, char** argv)
CHECK(my_type_set_translation(&t2, transl), RES_OK);
CHECK(my_type_set_translation(&t3, transl), RES_OK);
- d3(in_dir, 1, 0, 0);
+ d3(in_dir, 1, 0.1, 0);
CHECK(sanim_node_solve_pivot(&t2.node, in_dir), RES_OK);
CHECK(my_type_get_transform(&t3, transform), RES_OK);
d3(n, 0, 0, 1);
@@ -189,7 +187,7 @@ main(int argc, char** argv)
CHECK(my_type_set_translation(&t2, transl), RES_OK);
CHECK(my_type_set_translation(&t3, transl), RES_OK);
- d3(in_dir, 1, 0, 0);
+ d3(in_dir, 1, -0.5, 0);
CHECK(sanim_node_solve_pivot(&t2.node, in_dir), RES_OK);
CHECK(my_type_get_transform(&t3, transform), RES_OK);
d3(n, 0, 0, 1);
