commit 367b14d67390787601d8f90728848c5a9d2f43c4
parent 8fab1e25e39df3970e553021a590025684d60cac
Author: Christophe Coustet <christophe.coustet@meso-star.com>
Date: Mon, 17 Oct 2016 18:23:21 +0200
Continuing pivots; 1 axis tracking sun & 1 axis tracking output dir
Diffstat:
7 files changed, 526 insertions(+), 120 deletions(-)
diff --git a/src/sanim.h b/src/sanim.h
@@ -44,14 +44,14 @@
/* Forward declaration of external types */
struct logger;
struct mem_allocator;
-struct sanim_node_data;
+struct node_data;
/* Opaque Solstice Anim types */
struct sanim_device;
/* sanim_node type for building own node types */
struct sanim_node {
- struct sanim_node_data* data;
+ struct node_data* data;
};
/* types to describe pivots */
@@ -63,13 +63,14 @@ enum pivot_type {
};
struct sanim_pivot_1 {
- double ref_point[3];
- double ref_normal[3];
+ double ref_point[3]; /* in local space */
+ /* rotation axis is X */
+ double ref_normal[3]; /* in local space */
};
struct sanim_pivot_2 {
double spacing[3];
- double ref_point[3];
+ double ref_point[3]; /* in local space */
/* ref_normal is <0,1,0> */
};
@@ -85,7 +86,6 @@ struct sanim_pivot {
enum tracking_policy {
TRACKING_SUN, /* orient the device to face the sun */
TRACKING_POINT, /* direct the output flux towards a point */
- TRACKING_LINE, /* direct the output flux on a line */
TRACKING_OUT_DIR, /* direct the output flux towards a given dir */
TRACKING_TYPES_COUNT
@@ -96,16 +96,12 @@ struct sanim_policy_sun {
};
struct sanim_policy_point {
- double pos[3];
+ double target[3];
+ char target_is_local; /* is target in local space? */
};
-struct sanim_policy_line {
- double p1[3];
- double p2[3];
-};
-
-struct sanim_policy_dir {
- double u[3];
+struct sanim_policy_out_dir {
+ double u[3]; /* in world space */
};
struct sanim_tracking {
@@ -113,8 +109,7 @@ struct sanim_tracking {
union {
struct sanim_policy_sun sun;
struct sanim_policy_point point;
- struct sanim_policy_line line;
- struct sanim_policy_dir dir;
+ struct sanim_policy_out_dir out_dir;
} data;
};
BEGIN_DECLS
@@ -155,6 +150,11 @@ sanim_node_initialize_pivot
struct sanim_node* node);
SANIM_API res_T
+sanim_node_solve_pivot
+ (struct sanim_node* node,
+ const double in_dir[3]);
+
+SANIM_API res_T
sanim_node_release
(struct sanim_node* node);
@@ -174,9 +174,9 @@ sanim_node_set_rotations
const double rotations[3]); /* XYZ convention */
SANIM_API res_T
-sanim_node_get_world_transform
+sanim_node_get_transform
(struct sanim_node* node,
- double transform[12]); /* 3x4 column major matrix */
+ double transform[12]); /* 3x4 column major matrix */
END_DECLS
diff --git a/src/sanim_node.c b/src/sanim_node.c
@@ -20,56 +20,345 @@
#include <rsys/mem_allocator.h>
#include <rsys/ref_count.h>
#include <rsys/double33.h>
-#include <rsys/double44.h>
+
+#include <math.h>
/*******************************************************************************
* Helper functions
******************************************************************************/
static int
-is_ascendant
- (const struct sanim_node_data* data, const struct sanim_node_data* asc_data)
+is_ancestor
+ (const struct sanim_node* node, const struct sanim_node* possible_ancestor)
{
- ASSERT(data && asc_data);
- while (data) {
- if (data == asc_data) return 1;
- data = data->father;
+ ASSERT(node && node->data && possible_ancestor);
+ while (node) {
+ if (node == possible_ancestor) return 1;
+ node = node->data->father;
}
return 0;
}
static int
-is_after_pivot(const struct sanim_node_data* data) {
- ASSERT(data);
- while (data) {
- if (data->pivot) return 1;
- data = data->father;
+is_after_pivot(const struct sanim_node* node) {
+ ASSERT(node);
+ while (node) {
+ if (node->data->pivot_data) return 1;
+ node = node->data->father;
}
return 0;
}
+static void
+d34_muld34(double dst[12], const double a[12], const double b[12]) {
+ ASSERT(dst && a && b);
+ double tmp[3];
+ d3_add(dst + 9, d33_muld3(tmp, a, b + 9), a + 9);
+ d33_muld33(dst, a, b);
+}
+
+static void
+d34_set_identity(double dst[12]) {
+ ASSERT(dst);
+ d33_set_identity(dst);
+ d3_splat(dst + 9, 0);
+}
+
+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);
+ d33_rotation(tmp, SPLIT3(node->data->rotations));
+ d3_set(tmp + 9, node->data->translation);
+ d34_muld34(transform, tmp, transform);
+ }
+ else {
+ d33_rotation(transform, SPLIT3(node->data->rotations));
+ d3_set(transform + 9, node->data->translation);
+ }
+ return transform;
+}
+
static double*
-node_get_transform(struct sanim_node_data* data, double transform[16]) {
+compose_node_transform(struct sanim_node* node, double transform[12]) {
double tmp[12];
- ASSERT(data && transform);
- d33_rotation(tmp, SPLIT3(data->rotations));
- d3_set(transform, tmp);
- transform[3] = 0;
- d3_set(transform + 4, tmp + 3);
- transform[7] = 0;
- d3_set(transform + 8, tmp + 6);
- transform[11] = 0;
- d3_set(transform + 12, data->translation);
- transform[15] = 1;
+ 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);
+ d34_muld34(transform, tmp, transform);
+ }
+ d33_rotation(tmp, SPLIT3(node->data->rotations));
+ d3_set(tmp + 9, node->data->translation);
+ d34_muld34(transform, tmp, transform);
return transform;
}
-static void
-d33_setd44(double dst[12], double src[16]) {
- d3_set(dst, src);
- d3_set(dst + 3, src + 4);
- d3_set(dst + 6, src + 8);
- d3_set(dst + 9, src + 12);
- ASSERT(!src[3] && !src[7] && !src[11] && src[15] == 1);
+static void
+node_get_transform(struct sanim_node* node, const char include_own_pivot, double transform[12])
+{
+ struct sanim_node* ptr;
+ ASSERT(node && node->data && transform);
+ set_node_transform(node, include_own_pivot, transform);
+ ptr = node->data->father;
+ while (ptr) {
+ compose_node_transform(ptr, transform);
+ ptr = ptr->data->father;
+ }
+}
+
+static res_T
+compute_single_axis_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;
+ }
+
+ *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
+pivot_solve_single_axis_sun
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ double mat[12], inv[12];
+ double local_in[3], rotated_n[3];
+ const double* ref_normal;
+ double* angle;
+ ASSERT(node && in_dir);
+ ASSERT(node->data->pivot_data);
+ ASSERT(node->data->pivot_data->pivot.type == PIVOT_SINGLE_AXIS);
+ ASSERT(node->data->pivot_data->tracking.policy == TRACKING_SUN);
+ ASSERT(d3_is_normalized(in_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 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);
+
+ /* rotated_n = -local_in */
+ d3_muld(rotated_n, local_in, -1);
+
+ return compute_single_axis_angle(ref_normal, rotated_n, angle);
+}
+
+FINLINE res_T
+pivot_solve_single_axis_point
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ double mat[12], inv[12];
+ double local_in[3], rotated_n[3], local_target[3];
+ const double* ref_normal;
+ double* angle;
+ ASSERT(node && in_dir);
+ ASSERT(node->data->pivot_data);
+ ASSERT(node->data->pivot_data->pivot.type == PIVOT_SINGLE_AXIS);
+ ASSERT(node->data->pivot_data->tracking.policy == TRACKING_POINT);
+ ASSERT(d3_is_normalized(in_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 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);
+
+ /* get target point in local space */
+ if (node->data->pivot_data->tracking.data.point.target_is_local) {
+ 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);
+ }
+
+ /* compute rotated_n */
+
+
+ return compute_single_axis_angle(ref_normal, rotated_n, angle);
+}
+
+FINLINE res_T
+pivot_solve_single_axis_dir
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ double mat[12], inv[12];
+ double local_in[3], rotated_n[3], local_out[3];
+ const double* ref_normal;
+ double* angle;
+ ASSERT(node && in_dir);
+ ASSERT(node->data->pivot_data);
+ ASSERT(node->data->pivot_data->pivot.type == PIVOT_SINGLE_AXIS);
+ ASSERT(node->data->pivot_data->tracking.policy == TRACKING_OUT_DIR);
+ ASSERT(d3_is_normalized(in_dir));
+ ASSERT(d3_is_normalized(node->data->pivot_data->tracking.data.out_dir.u));
+
+ 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);
+
+ /* 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;
+ }
+
+ return compute_single_axis_angle(ref_normal, rotated_n, angle);
+}
+
+FINLINE res_T
+pivot_solve_single_axis
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ res_T res = RES_OK;
+ ASSERT(node && in_dir);
+ ASSERT(node->data->pivot_data);
+ ASSERT(node->data->pivot_data->pivot.type == PIVOT_SINGLE_AXIS);
+
+ switch (node->data->pivot_data->tracking.policy) {
+ case TRACKING_SUN:
+ res = pivot_solve_single_axis_sun(node, in_dir);
+ break;
+ case TRACKING_POINT:
+ res = pivot_solve_single_axis_point(node, in_dir);
+ break;
+ case TRACKING_OUT_DIR:
+ res = pivot_solve_single_axis_dir(node, in_dir);
+ break;
+ default: FATAL("Unreachable code.\n"); break;
+ }
+ ASSERT(node->data->pivot_data->pivot_angles[1] == 0
+ && node->data->pivot_data->pivot_angles[2] == 0);
+ return res;
+}
+
+FINLINE res_T
+pivot_solve_two_axis_sun
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ ASSERT(node && in_dir);
+ ASSERT(node->data->pivot_data);
+ ASSERT(node->data->pivot_data->pivot.type == PIVOT_TWO_AXIS);
+ ASSERT(node->data->pivot_data->tracking.policy == TRACKING_SUN);
+
+ return RES_OK;
+}
+
+FINLINE res_T
+pivot_solve_two_axis_point
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ ASSERT(node && in_dir);
+ ASSERT(node->data->pivot_data);
+ ASSERT(node->data->pivot_data->pivot.type == PIVOT_TWO_AXIS);
+ ASSERT(node->data->pivot_data->tracking.policy == TRACKING_POINT);
+
+ return RES_OK;
+}
+
+FINLINE res_T
+pivot_solve_two_axis_dir
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ ASSERT(node && in_dir);
+ ASSERT(node->data->pivot_data);
+ ASSERT(node->data->pivot_data->pivot.type == PIVOT_TWO_AXIS);
+ ASSERT(node->data->pivot_data->tracking.policy == TRACKING_OUT_DIR);
+
+ return RES_OK;
+}
+
+FINLINE res_T
+pivot_solve_two_axis
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ ASSERT(node && in_dir);
+ ASSERT(node->data->pivot_data);
+ ASSERT(node->data->pivot_data->pivot.type == PIVOT_TWO_AXIS);
+
+ switch (node->data->pivot_data->tracking.policy) {
+ case TRACKING_SUN:
+ return pivot_solve_two_axis_sun(node, in_dir);
+ case TRACKING_POINT:
+ return pivot_solve_two_axis_point(node, in_dir);
+ case TRACKING_OUT_DIR:
+ return pivot_solve_two_axis_dir(node, in_dir);
+ default: FATAL("Unreachable code.\n"); break;
+ }
+}
+
+FINLINE res_T
+copy_and_normalise_pivot_data
+ (struct pivot_data* dest,
+ const struct sanim_pivot* pivot,
+ const struct sanim_tracking* tracking)
+{
+ dest->pivot.type = pivot->type;
+ switch (pivot->type) {
+ case PIVOT_SINGLE_AXIS:
+ if (!d3_normalize(dest->pivot.data.pivot1.ref_normal, pivot->data.pivot1.ref_normal))
+ return RES_BAD_ARG;
+ if (dest->pivot.data.pivot1.ref_normal[0])
+ /* ref_normal not in the YZ plane */
+ return RES_BAD_ARG;
+ d3_set(dest->pivot.data.pivot1.ref_point, pivot->data.pivot1.ref_point);
+ break;
+ case PIVOT_TWO_AXIS:
+ d3_set(dest->pivot.data.pivot2.ref_point, pivot->data.pivot2.ref_point);
+ d3_set(dest->pivot.data.pivot2.spacing, pivot->data.pivot2.spacing);
+ break;
+ default: FATAL("Unreachable code.\n"); break;
+ }
+ dest->tracking.policy = tracking->policy;
+ switch (tracking->policy) {
+ case TRACKING_SUN:
+ /* nothing to be copied */
+ break;
+ case TRACKING_POINT:
+ d3_set(dest->tracking.data.point.target, tracking->data.point.target);
+ break;
+ case TRACKING_OUT_DIR:
+ if (!d3_normalize(dest->tracking.data.out_dir.u, tracking->data.out_dir.u))
+ return RES_BAD_ARG;
+ break;
+ default: FATAL("Unreachable code.\n"); break;
+ }
+ return RES_OK;
}
/*******************************************************************************
@@ -84,11 +373,11 @@ sanim_node_add_child
if (!node || !child) return RES_BAD_ARG;
if (child->data->father) return RES_BAD_ARG;
- if (is_ascendant(node->data, child->data)) return RES_BAD_ARG;
- if (child->data->pivot && is_after_pivot(node->data)) return RES_BAD_ARG;
+ if (is_ancestor(node, child)) return RES_BAD_ARG;
+ if (child->data->pivot_data && is_after_pivot(node)) return RES_BAD_ARG;
- child->data->father = node->data;
- res = darray_children_push_back(&node->data->children, &child->data);
+ child->data->father = node;
+ res = darray_children_push_back(&node->data->children, &child);
if (res != RES_OK) {
goto error;
}
@@ -113,7 +402,7 @@ sanim_node_initialize
if (!node) return RES_BAD_ARG;
alloc = allocator ? allocator : &mem_default_allocator;
- node->data = MEM_CALLOC(alloc, 1, sizeof(struct sanim_node_data));
+ node->data = MEM_CALLOC(alloc, 1, sizeof(struct node_data));
if (!node->data) {
res = RES_MEM_ERR;
goto error;
@@ -147,18 +436,14 @@ sanim_node_initialize_pivot
if (res != RES_OK) goto error;
alloc = allocator ? allocator : &mem_default_allocator;
- node->data->pivot = MEM_CALLOC(alloc, 1, sizeof(struct sanim_pivot));
- if (!node->data->pivot) {
+ node->data->pivot_data = MEM_CALLOC(alloc, 1, sizeof(struct pivot_data));
+ if (!node->data->pivot_data) {
res = RES_MEM_ERR;
goto error;
}
- node->data->tracking = MEM_CALLOC(alloc, 1, sizeof(struct sanim_tracking));
- if (!node->data->tracking) {
- res = RES_MEM_ERR;
- goto error;
- }
- *node->data->pivot = *pivot;
- *node->data->tracking = *tracking;
+
+ res = copy_and_normalise_pivot_data(node->data->pivot_data, pivot, tracking);
+ if (res != RES_OK) goto error;
exit:
return res;
@@ -168,17 +453,36 @@ error:
}
res_T
+sanim_node_solve_pivot
+ (struct sanim_node* node,
+ const double in_dir[3])
+{
+ double dir[3];
+ if (!node || !in_dir) return RES_BAD_ARG;
+ if (!node->data->pivot_data) return RES_BAD_ARG;
+ if (!d3_normalize(dir, in_dir)) return RES_BAD_ARG;
+
+ switch (node->data->pivot_data->pivot.type) {
+ case PIVOT_SINGLE_AXIS:
+ return pivot_solve_single_axis(node, dir);
+ break;
+ case PIVOT_TWO_AXIS:
+ pivot_solve_two_axis(node, dir);
+ break;
+ default: FATAL("Unreachable code.\n"); break;
+ }
+ return RES_OK;
+}
+
+res_T
sanim_node_release
(struct sanim_node* node)
{
if (!node) return RES_BAD_ARG;
if (node->data) {
darray_children_release(&node->data->children);
- if (node->data->pivot) {
- MEM_RM(node->data->allocator, node->data->pivot);
- }
- if (node->data->tracking) {
- MEM_RM(node->data->allocator, node->data->tracking);
+ if (node->data->pivot_data) {
+ MEM_RM(node->data->allocator, node->data->pivot_data);
}
MEM_RM(node->data->allocator, node->data);
}
@@ -206,20 +510,10 @@ sanim_node_set_rotations
}
res_T
-sanim_node_get_world_transform
- (struct sanim_node* node,
- double transform[12])
+sanim_node_get_transform(struct sanim_node* node, double transform[12])
{
- double world[16], tmp[16]; /* 4x4 column major matrix */
- struct sanim_node_data* ptr;
- if (!node || !node->data || !transform) return RES_BAD_ARG;
- node_get_transform(node->data, world);
- ptr = node->data->father;
- while (ptr) {
- node_get_transform(ptr, tmp);
- d44_muld44(world, tmp, world);
- ptr = ptr->father;
- }
- d33_setd44(transform, world);
+ if (!node || !node->data || !transform)
+ return RES_BAD_ARG;
+ node_get_transform(node, 1, transform);
return RES_OK;
-}
+}
+\ No newline at end of file
diff --git a/src/sanim_node_c.h b/src/sanim_node_c.h
@@ -17,23 +17,30 @@
#define SANIM_NODE_C_H
struct mem_allocator;
-struct sanim_pivot;
+
+#include "sanim.h"
#include <rsys/dynamic_array.h>
/* Define the darray_children data structure */
#define DARRAY_NAME children
-#define DARRAY_DATA struct sanim_node_data*
+#define DARRAY_DATA struct sanim_node*
#include <rsys/dynamic_array.h>
-struct sanim_node_data {
+/* supplemental data for nodes that are pivots */
+struct pivot_data {
+ double pivot_angles[3];
+ struct sanim_pivot pivot;
+ struct sanim_tracking tracking;
+};
+
+struct node_data {
double translation[3];
double rotations[3];
- struct sanim_node_data* father; /* can be NULL: root node */
+ struct sanim_node* father; /* can be NULL: root node */
struct darray_children children;
struct mem_allocator* allocator;
- struct sanim_pivot* pivot;
- struct sanim_tracking* tracking;
+ struct pivot_data* pivot_data;
};
diff --git a/src/test_sanim_node_pivot.c b/src/test_sanim_node_pivot.c
@@ -23,45 +23,110 @@ main(int argc, char** argv)
{
struct mem_allocator allocator;
struct my_type t1, t2, t3;
- struct sanim_pivot pivot;
+ struct sanim_pivot pivot1;
struct sanim_tracking tracking;
- double transl[3], rot[3], transform[12];
+ double transform[12];
+ double transl[3], rot[3], in_dir[3], n[3], tmp[3];
(void) argc, (void) argv;
mem_init_proxy_allocator(&allocator, &mem_default_allocator);
-
- /* test a typical use in my_type */
+
tracking.policy = TRACKING_SUN;
- pivot.type = PIVOT_SINGLE_AXIS;
- d3_splat(pivot.data.pivot1.ref_point, 0);
- d3(pivot.data.pivot1.ref_normal, 0, 1, 0);
+ 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);
+ /* ref_normal not in the YZ plane */
+ 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);
+ CHECK(my_type_release(&t1), RES_OK);
+ CHECK(my_type_init_pivot(NULL, &pivot1, &tracking, &t1), RES_OK);
+ CHECK(my_type_release(&t1), RES_OK);
+
CHECK(my_type_init(&allocator, &t1), RES_OK);
- CHECK(my_type_init_pivot(&allocator, &pivot, &tracking, &t2), 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);
d3_splat(transl, +1);
+ d3(rot, 0, 0, PI/2);
CHECK(my_type_set_translation(&t1, transl), RES_OK);
-
- d3_splat(transl, -1);
+ 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);
- CHECK(my_type_get_world_transform(&t2, transform), RES_OK);
- CHECK(d33_is_identity(transform), 1);
- CHECK(d3_is_zero(transform + 9), 1);
+ d3(in_dir, 0, 0.99, -0.1);
+ /* rotation axis is Y after positioning: cannot accomodate in_dir */
+ CHECK(sanim_node_solve_pivot(&t2.node, in_dir), RES_BAD_ARG);
- d3(rot, PI, 0, 0);
+ d3(in_dir, 1, 0, -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);
+ CHECK(d3_eq_eps(transform + 9, d3(tmp, -sqrt(2), 3, 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);
+
+ tracking.policy = TRACKING_OUT_DIR;
+ pivot1.type = PIVOT_SINGLE_AXIS;
+ d3(pivot1.data.pivot1.ref_normal, 0, 0, 1);
+ d3(tracking.data.out_dir.u, 0, 1, 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_rotations(&t2, rot), RES_OK);
- d3(transl, 0, +1, 0);
+ CHECK(my_type_set_translation(&t2, transl), RES_OK);
+ CHECK(my_type_set_translation(&t3, transl), RES_OK);
+
+ d3(in_dir, 0, -1, -0.1);
+ /* rotation axis is Y after positioning: cannot accomodate <in_dir,out_dir> */
+ CHECK(sanim_node_solve_pivot(&t2.node, in_dir), RES_BAD_ARG);
+
+ CHECK(my_type_release(&t1), RES_OK);
+ CHECK(my_type_release(&t2), RES_OK);
+ CHECK(my_type_release(&t3), RES_OK);
+
+ tracking.policy = TRACKING_OUT_DIR;
+ pivot1.type = PIVOT_SINGLE_AXIS;
+ d3(pivot1.data.pivot1.ref_normal, 0, 0, 1);
+ d3(tracking.data.out_dir.u, 1, 0, 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, -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);
- CHECK(my_type_get_world_transform(&t2, transform), RES_OK);
- CHECK(d33_is_identity_eps(transform, 1e-10), 1);
- CHECK(d3_is_zero_eps(transform + 9, 1e-10), 1);
+ tracking.data.point.target_is_local = 0;
CHECK(my_type_release(&t1), RES_OK);
CHECK(my_type_release(&t2), RES_OK);
diff --git a/src/test_sanim_node_transform.c b/src/test_sanim_node_transform.c
@@ -23,7 +23,8 @@ main(int argc, char** argv)
{
struct mem_allocator allocator;
struct my_type t1, t2, t3, t;
- double transl[3], rot[3], transform[12], transform2[12];
+ double transl[3], rot[3];
+ double transform[12], transform_[12];
(void) argc, (void) argv;
mem_init_proxy_allocator(&allocator, &mem_default_allocator);
@@ -51,9 +52,9 @@ main(int argc, char** argv)
d3_splat(transl, -1);
CHECK(my_type_set_translation(&t2, transl), RES_OK);
- CHECK(my_type_get_world_transform(NULL, transform), RES_BAD_ARG);
- CHECK(my_type_get_world_transform(&t2, NULL), RES_BAD_ARG);
- CHECK(my_type_get_world_transform(&t2, transform), RES_OK);
+ CHECK(my_type_get_transform(NULL, transform), RES_BAD_ARG);
+ CHECK(my_type_get_transform(&t2, NULL), RES_BAD_ARG);
+ CHECK(my_type_get_transform(&t2, transform), RES_OK);
CHECK(d33_is_identity(transform), 1);
CHECK(d3_is_zero(transform + 9), 1);
@@ -66,19 +67,56 @@ main(int argc, char** argv)
CHECK(my_type_set_translation(&t1, transl), RES_OK);
CHECK(my_type_set_translation(&t2, transl), RES_OK);
- CHECK(my_type_get_world_transform(&t2, transform), RES_OK);
+ CHECK(my_type_get_transform(&t2, transform), RES_OK);
CHECK(d33_is_identity_eps(transform, 1e-10), 1);
CHECK(d3_is_zero_eps(transform + 9, 1e-10), 1);
- /* check 1 node with 3 rotations VS 3 chained nodes with 1 rotation each */
- d3(rot, 0.17, -0.52, 0.31);
+ /* check translation & 1 rotation in a single node VS in 2 chained nodes */
+ d3(rot, 0.17, 0, 0);
d3(transl, 0.3, 2, -1);
CHECK(my_type_init(&allocator, &t), RES_OK);
CHECK(my_type_set_translation(&t, transl), RES_OK);
CHECK(my_type_set_rotations(&t, rot), RES_OK);
- CHECK(my_type_get_world_transform(&t, transform), RES_OK);
+ CHECK(my_type_get_transform(&t, transform), RES_OK);
CHECK(my_type_init(&allocator, &t3), RES_OK);
CHECK(my_type_add_child(&t2, &t3), RES_OK);
+ d3(rot, 0, 0, 0);
+ CHECK(my_type_set_translation(&t1, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t1, rot), RES_OK);
+ d3(transl, 0, 0, 0);
+ d3(rot, 0.17, 0, 0);
+ CHECK(my_type_set_translation(&t2, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t2, rot), RES_OK);
+ CHECK(my_type_get_transform(&t2, transform_), RES_OK);
+ CHECK(d33_eq_eps(transform, transform_, 1e-10), 1);
+ CHECK(d3_eq_eps(transform + 9, transform_ + 9, 1e-10), 1);
+
+ /* check translation & 2 rotations in a single node VS in 2 chained nodes */
+ d3(rot, 0.17, -0.52, 0);
+ d3(transl, 0.3, 2, -1);
+ CHECK(my_type_set_translation(&t, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t, rot), RES_OK);
+ CHECK(my_type_get_transform(&t, transform), RES_OK);
+ d3(rot, 0, 0, 0);
+ CHECK(my_type_set_translation(&t1, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t1, rot), RES_OK);
+ d3(transl, 0, 0, 0);
+ d3(rot, 0.17, 0, 0);
+ CHECK(my_type_set_translation(&t2, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t2, rot), RES_OK);
+ d3(rot, 0, -0.52, 0);
+ CHECK(my_type_set_translation(&t3, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t3, rot), RES_OK);
+ CHECK(my_type_get_transform(&t3, transform_), RES_OK);
+ CHECK(d33_eq_eps(transform, transform_, 1e-10), 1);
+ CHECK(d3_eq_eps(transform + 9, transform_ + 9, 1e-10), 1);
+
+ /* check 1 node with 3 rotations VS 3 chained nodes with 1 rotation each */
+ d3(rot, 0.17, -0.52, 0.31);
+ d3(transl, 0.3, 2, -1);
+ CHECK(my_type_set_translation(&t, transl), RES_OK);
+ CHECK(my_type_set_rotations(&t, rot), RES_OK);
+ CHECK(my_type_get_transform(&t, transform), RES_OK);
d3(rot, 0.17, 0, 0);
CHECK(my_type_set_translation(&t1, transl), RES_OK);
CHECK(my_type_set_rotations(&t1, rot), RES_OK);
@@ -89,8 +127,9 @@ main(int argc, char** argv)
d3(rot, 0, 0, 0.31);
CHECK(my_type_set_translation(&t3, transl), RES_OK);
CHECK(my_type_set_rotations(&t3, rot), RES_OK);
- CHECK(my_type_get_world_transform(&t3, transform2), RES_OK);
- CHECK(d33_eq_eps(transform, transform2, 1e-10), 1);
+ CHECK(my_type_get_transform(&t3, transform_), RES_OK);
+ CHECK(d33_eq_eps(transform, transform_, 1e-10), 1);
+ CHECK(d3_eq_eps(transform + 9, transform_ + 9, 1e-10), 1);
CHECK(my_type_release(&t1), RES_OK);
CHECK(my_type_release(&t2), RES_OK);
diff --git a/src/test_sanim_utils.c b/src/test_sanim_utils.c
@@ -72,9 +72,9 @@ my_type_set_rotations(struct my_type* t, const double rotations[3]) {
}
res_T
-my_type_get_world_transform(struct my_type* t, double transform[12]) {
+my_type_get_transform(struct my_type* t, double transform[12]) {
if (!t || !transform) return RES_BAD_ARG;
- return sanim_node_get_world_transform(&t->node, transform);
+ return sanim_node_get_transform(&t->node, transform);
}
char
diff --git a/src/test_sanim_utils.h b/src/test_sanim_utils.h
@@ -54,7 +54,7 @@ res_T
my_type_set_rotations(struct my_type* t, const double rotations[3]);
res_T
-my_type_get_world_transform(struct my_type* t, double transform[12]);
+my_type_get_transform(struct my_type* t, double transform[12]);
/*******************************************************************************
* Utilities
