|
1
|
1 #include <stdio.h>
|
|
|
2 #include <string.h>
|
|
|
3
|
|
|
4 typedef struct coord {
|
|
|
5 int seq;
|
|
|
6 float matrix[6];
|
|
|
7 float aggr_matrix[6];
|
|
|
8 struct coord *parent;
|
|
|
9 struct coord *children, *sibling;
|
|
|
10 } coord_t;
|
|
|
11
|
|
|
12 static void mul_matrix(float *m1, float *m2, float *dst) {
|
|
|
13 dst[0] = m1[0] * m2[0] + m1[1] * m2[3];
|
|
|
14 dst[1] = m1[0] * m2[1] + m1[1] * m2[4];
|
|
|
15 dst[2] = m1[0] * m2[2] + m1[1] * m2[5] + m1[2];
|
|
|
16 dst[3] = m1[3] * m2[0] + m1[4] * m2[3];
|
|
|
17 dst[4] = m1[3] * m2[1] + m1[4] * m2[4];
|
|
|
18 dst[5] = m1[3] * m2[2] + m1[4] * m2[5] + m1[5];
|
|
|
19 }
|
|
|
20
|
|
|
21 static void compute_transform_function(coord_t *visit) {
|
|
|
22 if(visit->parent)
|
|
|
23 mul_matrix(visit->parent->aggr_matrix,
|
|
|
24 visit->matrix, visit->aggr_matrix);
|
|
|
25 else
|
|
|
26 memcpy(visit->aggr_matrix, visit->matrix, sizeof(visit->matrix));
|
|
|
27 }
|
|
|
28
|
|
|
29 /*! \brief Update aggregate matrices of elements under a sub-tree.
|
|
|
30 *
|
|
|
31 * A subtree is specified by the root of it. All elements in the subtree
|
|
|
32 * are effected by that changes of matrix of the subtree root.
|
|
|
33 */
|
|
|
34 void update_aggr_matrix(coord_t *start) {
|
|
|
35 coord_t *visit, *child, *next;
|
|
|
36
|
|
|
37 compute_transform_function(start);
|
|
|
38
|
|
|
39 visit = start;
|
|
|
40 while(visit) {
|
|
|
41 child = visit->children;
|
|
|
42 while(child) {
|
|
|
43 compute_transform_function(child);
|
|
|
44 child = child->sibling;
|
|
|
45 }
|
|
|
46
|
|
|
47 if(visit->children)
|
|
|
48 visit = visit->children;
|
|
|
49 else if(visit->sibling)
|
|
|
50 visit = visit->sibling;
|
|
|
51 else {
|
|
|
52 next = NULL;
|
|
|
53 while(visit->parent && visit->parent != start) {
|
|
|
54 visit = visit->parent;
|
|
|
55 if(visit->sibling) {
|
|
|
56 next = visit->sibling;
|
|
|
57 break;
|
|
|
58 }
|
|
|
59 }
|
|
|
60 visit = next;
|
|
|
61 }
|
|
|
62 }
|
|
|
63 }
|
|
|
64
|
|
|
65 void coord_init(coord_t *co, coord_t *parent) {
|
|
|
66 memset(co, 0, sizeof(coord_t));
|
|
|
67 if(parent) {
|
|
|
68 co->parent = parent;
|
|
|
69 co->sibling = parent->children;
|
|
|
70 parent->children = co;
|
|
|
71 }
|
|
|
72 co->matrix[0] = 1;
|
|
|
73 co->matrix[4] = 1;
|
|
|
74 }
|
|
|
75
|
|
|
76 #ifdef UNITTEST
|
|
|
77
|
|
|
78 #include <CUnit/Basic.h>
|
|
|
79
|
|
|
80 void test_update_aggr_matrix(void) {
|
|
|
81 coord_t elms[6];
|
|
|
82
|
|
|
83 coord_init(elms, NULL);
|
|
|
84 coord_init(elms + 1, elms);
|
|
|
85 coord_init(elms + 2, elms);
|
|
|
86 coord_init(elms + 3, elms + 1);
|
|
|
87 coord_init(elms + 4, elms + 1);
|
|
|
88 coord_init(elms + 5, elms + 2);
|
|
|
89
|
|
|
90 /* | 2 -1 0 |
|
|
|
91 * | 0 1 0 |
|
|
|
92 * | 0 0 1 |
|
|
|
93 */
|
|
|
94 elms[0].matrix[0] = 2;
|
|
|
95 elms[0].matrix[1] = -1;
|
|
|
96
|
|
|
97 /* | 1 3 0 |
|
|
|
98 * | 5 1 0 |
|
|
|
99 * | 0 0 1 |
|
|
|
100 */
|
|
|
101 elms[1].matrix[1] = 3;
|
|
|
102 elms[1].matrix[3] = 5;
|
|
|
103
|
|
|
104 update_aggr_matrix(elms);
|
|
|
105
|
|
|
106 /* | -3 5 0 |
|
|
|
107 * | 5 1 0 |
|
|
|
108 * | 0 0 1 |
|
|
|
109 */
|
|
|
110 CU_ASSERT(elms[3].aggr_matrix[0] == -3);
|
|
|
111 CU_ASSERT(elms[3].aggr_matrix[1] == 5);
|
|
|
112 CU_ASSERT(elms[3].aggr_matrix[2] == 0);
|
|
|
113 CU_ASSERT(elms[3].aggr_matrix[3] == 5);
|
|
|
114 CU_ASSERT(elms[3].aggr_matrix[4] == 1);
|
|
|
115 CU_ASSERT(elms[3].aggr_matrix[5] == 0);
|
|
|
116
|
|
|
117 CU_ASSERT(elms[4].aggr_matrix[0] == -3);
|
|
|
118 CU_ASSERT(elms[4].aggr_matrix[1] == 5);
|
|
|
119 CU_ASSERT(elms[4].aggr_matrix[2] == 0);
|
|
|
120 CU_ASSERT(elms[4].aggr_matrix[3] == 5);
|
|
|
121 CU_ASSERT(elms[4].aggr_matrix[4] == 1);
|
|
|
122 CU_ASSERT(elms[4].aggr_matrix[5] == 0);
|
|
|
123
|
|
|
124 CU_ASSERT(elms[5].aggr_matrix[0] == 2);
|
|
|
125 CU_ASSERT(elms[5].aggr_matrix[1] == -1);
|
|
|
126 CU_ASSERT(elms[5].aggr_matrix[2] == 0);
|
|
|
127 CU_ASSERT(elms[5].aggr_matrix[3] == 0);
|
|
|
128 CU_ASSERT(elms[5].aggr_matrix[4] == 1);
|
|
|
129 CU_ASSERT(elms[5].aggr_matrix[5] == 0);
|
|
|
130 }
|
|
|
131
|
|
|
132 CU_pSuite get_coord_suite(void) {
|
|
|
133 CU_pSuite suite;
|
|
|
134
|
|
|
135 suite = CU_add_suite("Suite_coord", NULL, NULL);
|
|
|
136 CU_ADD_TEST(suite, test_update_aggr_matrix);
|
|
|
137
|
|
|
138 return suite;
|
|
|
139 }
|
|
|
140
|
|
|
141 #endif
|
