commit 297250ada07784787724b90a135c349752497744
parent 367b14d67390787601d8f90728848c5a9d2f43c4
Author: Christophe Coustet <christophe.coustet@meso-star.com>
Date: Tue, 18 Oct 2016 15:43:35 +0200
Add 1 axis pivot tracking a target line
Diffstat:
3 files changed, 134 insertions(+), 26 deletions(-)
diff --git a/src/sanim.h b/src/sanim.h
@@ -62,10 +62,10 @@ enum pivot_type {
PIVOT_TYPES_COUNT__
};
+/* pivot with X rotation axis */
struct sanim_pivot_1 {
- double ref_point[3]; /* in local space */
- /* rotation axis is X */
- double ref_normal[3]; /* in local space */
+ double ref_point[3]; /* in post-pivot local space */
+ double ref_normal[3]; /* normal without rotation; used to compute output dir */
};
struct sanim_pivot_2 {
@@ -96,8 +96,10 @@ struct sanim_policy_sun {
};
struct sanim_policy_point {
+ /* target can be in local space (ie in the same system than the pivot)
+ * or in world space */
double target[3];
- char target_is_local; /* is target in local space? */
+ char target_is_local;
};
struct sanim_policy_out_dir {
diff --git a/src/sanim_node.c b/src/sanim_node.c
@@ -64,12 +64,19 @@ d34_set_identity(double dst[12]) {
}
static double*
+set_pivot_transform(const double angles[3], double transform[12]) {
+ ASSERT(angles && transform);
+ d33_rotation(transform, SPLIT3(angles));
+ d3_splat(transform + 9, 0);
+ return transform;
+}
+
+static double*
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) {
- d33_rotation(transform, SPLIT3(node->data->pivot_data->pivot_angles));
- d3_splat(transform + 9, 0);
+ set_pivot_transform(node->data->pivot_data->pivot_angles, transform);
d33_rotation(tmp, SPLIT3(node->data->rotations));
d3_set(tmp + 9, node->data->translation);
d34_muld34(transform, tmp, transform);
@@ -86,8 +93,7 @@ compose_node_transform(struct sanim_node* node, double transform[12]) {
double tmp[12];
ASSERT(node && node->data && transform);
if (node->data->pivot_data) {
- d33_rotation(tmp, SPLIT3(node->data->pivot_data->pivot_angles));
- d3_splat(tmp + 9, 0);
+ set_pivot_transform(node->data->pivot_data->pivot_angles, tmp);
d34_muld34(transform, tmp, transform);
}
d33_rotation(tmp, SPLIT3(node->data->rotations));
@@ -123,7 +129,7 @@ compute_single_axis_angle
/* cannot be obtained by X-rotate */
return RES_BAD_ARG;
}
-
+ /* solve in the YZ plane */
*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]);
@@ -157,19 +163,22 @@ pivot_solve_single_axis_sun
/* 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);
}
FINLINE res_T
-pivot_solve_single_axis_point
+pivot_solve_single_axis_line
(struct sanim_node* node,
const double in_dir[3])
{
- double mat[12], inv[12];
- double local_in[3], rotated_n[3], local_target[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;
- double* angle;
+ double angles[3], previous_angle, delta;
+ int cpt = 0;
ASSERT(node && in_dir);
ASSERT(node->data->pivot_data);
ASSERT(node->data->pivot_data->pivot.type == PIVOT_SINGLE_AXIS);
@@ -178,7 +187,7 @@ pivot_solve_single_axis_point
ref_normal = node->data->pivot_data->pivot.data.pivot1.ref_normal;
ASSERT(d3_is_normalized(ref_normal));
- angle = &node->data->pivot_data->pivot_angles[0]; /* X-rotate */
+ d3_set(ref_point, node->data->pivot_data->pivot.data.pivot1.ref_point);
/* get in_dir in local space */
node_get_transform(node, 0, mat);
@@ -190,14 +199,40 @@ pivot_solve_single_axis_point
d3_set(local_target, node->data->pivot_data->tracking.data.point.target);
}
else {
- d33_muld3(local_target, inv, node->data->pivot_data->tracking.data.point.target);
- d3_add(local_target, local_target, inv + 9);
+ d3_sub(local_target, node->data->pivot_data->tracking.data.point.target, mat + 9);
+ d33_muld3(local_target, inv, local_target);
}
- /* compute rotated_n */
-
-
- return compute_single_axis_angle(ref_normal, rotated_n, angle);
+ /* TODO: get rid of iterative solving */
+ d3_splat(angles, 0);
+ do {
+ 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 */
+ return RES_BAD_ARG;
+ }
+ /* compute 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 */
+ return RES_BAD_ARG;
+ }
+ previous_angle = angles[0];
+ res = compute_single_axis_angle(ref_normal, rotated_n, angles); /* X-rotate */
+ if (res != RES_OK) return res;
+ delta = fabs(previous_angle - angles[0]);
+ if (delta < 1e-10 || ++cpt == 25) break;
+ set_pivot_transform(angles, pivot);
+ /* update ref_point */
+ d33_muld3(ref_point, pivot, node->data->pivot_data->pivot.data.pivot1.ref_point);
+ /* no d3_add(ref_point, ref_point, pivot + 9) as pivot has no offset to add */
+ } while (1);
+
+ ASSERT(angles[1] == 0 && angles[2] == 0);
+ d3_set(node->data->pivot_data->pivot_angles, angles);
+ return RES_OK;
}
FINLINE res_T
@@ -218,13 +253,13 @@ pivot_solve_single_axis_dir
ref_normal = node->data->pivot_data->pivot.data.pivot1.ref_normal;
ASSERT(d3_is_normalized(ref_normal));
- angle = &node->data->pivot_data->pivot_angles[0]; /* X-rotate */
/* get in_dir and out_dir in local space */
node_get_transform(node, 0, mat);
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 */
d3_sub(rotated_n, local_out, local_in);
@@ -233,6 +268,7 @@ pivot_solve_single_axis_dir
return RES_BAD_ARG;
}
+ angle = &node->data->pivot_data->pivot_angles[0]; /* X-rotate */
return compute_single_axis_angle(ref_normal, rotated_n, angle);
}
@@ -251,7 +287,8 @@ pivot_solve_single_axis
res = pivot_solve_single_axis_sun(node, in_dir);
break;
case TRACKING_POINT:
- res = pivot_solve_single_axis_point(node, in_dir);
+ /* track the X line that includes ref_point */
+ res = pivot_solve_single_axis_line(node, in_dir);
break;
case TRACKING_OUT_DIR:
res = pivot_solve_single_axis_dir(node, in_dir);
@@ -351,6 +388,7 @@ copy_and_normalise_pivot_data
break;
case TRACKING_POINT:
d3_set(dest->tracking.data.point.target, tracking->data.point.target);
+ dest->tracking.data.point.target_is_local = tracking->data.point.target_is_local;
break;
case TRACKING_OUT_DIR:
if (!d3_normalize(dest->tracking.data.out_dir.u, tracking->data.out_dir.u))
diff --git a/src/test_sanim_node_pivot.c b/src/test_sanim_node_pivot.c
@@ -31,14 +31,18 @@ main(int argc, char** argv)
mem_init_proxy_allocator(&allocator, &mem_default_allocator);
- tracking.policy = TRACKING_SUN;
- pivot1.type = PIVOT_SINGLE_AXIS;
- d3(pivot1.data.pivot1.ref_normal, 1, 0, 1);
-
CHECK(my_type_init_pivot(&allocator, NULL, &tracking, &t1), RES_BAD_ARG);
CHECK(my_type_init_pivot(&allocator, &pivot1, NULL, &t1), RES_BAD_ARG);
CHECK(my_type_init_pivot(&allocator, &pivot1, &tracking, NULL), RES_BAD_ARG);
+
+ /* 1 axis tracking sun */
+
+ tracking.policy = TRACKING_SUN;
+ pivot1.type = PIVOT_SINGLE_AXIS;
+ d3(pivot1.data.pivot1.ref_normal, 0, 0, 1);
+
/* ref_normal not in the YZ plane */
+ d3(pivot1.data.pivot1.ref_normal, 1, 0, 1);
CHECK(my_type_init_pivot(&allocator, &pivot1, &tracking, &t1), RES_BAD_ARG);
d3(pivot1.data.pivot1.ref_normal, 0, 0, 1);
CHECK(my_type_init_pivot(&allocator, &pivot1, &tracking, &t1), RES_OK);
@@ -76,6 +80,8 @@ main(int argc, char** argv)
CHECK(my_type_release(&t2), RES_OK);
CHECK(my_type_release(&t3), RES_OK);
+ /* 1 axis tracking with a fixed output dir */
+
tracking.policy = TRACKING_OUT_DIR;
pivot1.type = PIVOT_SINGLE_AXIS;
d3(pivot1.data.pivot1.ref_normal, 0, 0, 1);
@@ -126,8 +132,70 @@ main(int argc, char** argv)
CHECK(d3_eq_eps(n, tmp, 1e-10), 1);
CHECK(d3_eq_eps(transform + 9, d3(tmp, -1, 3, 3), 1e-10), 1);
+ CHECK(my_type_release(&t1), RES_OK);
+ CHECK(my_type_release(&t2), RES_OK);
+ CHECK(my_type_release(&t3), RES_OK);
+
+ /* 1 axis tracking a target point */
+
+ tracking.policy = TRACKING_POINT;
+ pivot1.type = PIVOT_SINGLE_AXIS;
+ d3(pivot1.data.pivot1.ref_normal, 0, 0, 1);
+ d3(pivot1.data.pivot1.ref_point, 0, 0, 10 * sqrt(2));
+ d3(tracking.data.point.target, 0, 10, 30);
+ tracking.data.point.target_is_local = 1;
+
+ CHECK(my_type_init(&allocator, &t1), RES_OK);
+ CHECK(my_type_init_pivot(&allocator, &pivot1, &tracking, &t2), RES_OK);
+ CHECK(my_type_init(&allocator, &t3), RES_OK);
+
+ CHECK(my_type_add_child(&t1, &t2), RES_OK);
+ CHECK(my_type_add_child(&t2, &t3), RES_OK);
+
+ CHECK(my_type_set_translation(&t1, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t1, rot), RES_OK);
+ CHECK(my_type_set_translation(&t2, transl), RES_OK);
+ CHECK(my_type_set_translation(&t3, transl), RES_OK);
+
+ d3(in_dir, 1, 0, 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);
+ d33_muld3(n, transform, n);
+ CHECK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-10), 1);
+
+ CHECK(my_type_release(&t1), RES_OK);
+ CHECK(my_type_release(&t2), RES_OK);
+ CHECK(my_type_release(&t3), RES_OK);
+
+ /* same 1 axis tracking with a non-local target point */
+
+ tracking.policy = TRACKING_POINT;
+ pivot1.type = PIVOT_SINGLE_AXIS;
+ d3(pivot1.data.pivot1.ref_normal, 0, 0, 1);
+ d3(pivot1.data.pivot1.ref_point, 0, 0, 10 * sqrt(2));
+ d3(tracking.data.point.target, -10, 2, 32);
tracking.data.point.target_is_local = 0;
+ CHECK(my_type_init(&allocator, &t1), RES_OK);
+ CHECK(my_type_init_pivot(&allocator, &pivot1, &tracking, &t2), RES_OK);
+ CHECK(my_type_init(&allocator, &t3), RES_OK);
+
+ CHECK(my_type_add_child(&t1, &t2), RES_OK);
+ CHECK(my_type_add_child(&t2, &t3), RES_OK);
+
+ CHECK(my_type_set_translation(&t1, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t1, rot), RES_OK);
+ CHECK(my_type_set_translation(&t2, transl), RES_OK);
+ CHECK(my_type_set_translation(&t3, transl), RES_OK);
+
+ d3(in_dir, 1, 0, 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);
+ d33_muld3(n, transform, n);
+ CHECK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-10), 1);
+
CHECK(my_type_release(&t1), RES_OK);
CHECK(my_type_release(&t2), RES_OK);
CHECK(my_type_release(&t3), RES_OK);
