~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/math/transform/transform3.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /**@(#)Transforms
  2  * @(#)Transform3 is {int type; struct mat3 R; struct vec3 t;}
  3 **/
  4 
  5 #include <stdio.h>
  6 #include <math.h>
  7 #include <tina/sys.h>
  8 #include <tina/sysfuncs.h>
  9 #include <tina/math.h>
 10 #include <tina/mathfuncs.h>
 11 
 12 Transform3 trans3(Mat3 R, Vec3 t)
 13 {
 14     Transform3 transf = {Transform3_id};
 15 
 16     transf.R = R;
 17     transf.t = t;
 18     return (transf);
 19 }
 20 
 21 Transform3 *trans3_alloc(void)
 22 {
 23     Transform3 *transf = ts_ralloc(Transform3);
 24 
 25     transf->R = mat3_unit();
 26     transf->t = vec3_zero();
 27     return (transf);
 28 }
 29 
 30 Transform3 *trans3_make(Mat3 R, Vec3 t)
 31 {
 32     Transform3 *transf = ts_ralloc(Transform3);
 33 
 34     transf->R = R;
 35     transf->t = t;
 36     return (transf);
 37 }
 38 
 39 Transform3 *trans3_copy(Transform3 * transf)
 40 {
 41     if (transf == NULL)
 42         return (NULL);
 43 
 44     return (trans3_make(transf->R, transf->t));
 45 }
 46 
 47 void    trans3_free(void *transf)
 48 {
 49     rfree((void *) transf);
 50 }
 51 
 52 Transform3 trans3_unit(void)
 53 {
 54     Transform3 transf = {Transform3_id};
 55 
 56     transf.R = mat3_unit();
 57     transf.t = vec3_zero();
 58     return (transf);
 59 }
 60 
 61 Transform3 trans3_prod(Transform3 transf2, Transform3 transf1)
 62 {
 63     Transform3 prod = {Transform3_id};
 64 
 65     prod.R = mat3_prod(transf2.R, transf1.R);
 66     prod.t = vec3_sum(transf2.t, mat3_vprod(transf2.R, transf1.t));
 67     return (prod);
 68 }
 69 
 70 Transform3 trans3_inverse(Transform3 transf)
 71 {
 72     Transform3 inv = {Transform3_id};
 73 
 74     inv.R = mat3_transpose(transf.R);
 75     inv.t = mat3_vprod(inv.R, vec3_minus(transf.t));
 76     return (inv);
 77 }
 78 
 79 Vec3    trans3_pos(Transform3 transf, Vec3 v)
 80 {
 81     return (vec3_sum(mat3_vprod(transf.R, v), transf.t));
 82 }
 83 
 84 Vec3    trans3_inverse_pos(Transform3 transf, Vec3 v)
 85 {
 86     return (mat3_transpose_vprod(transf.R, vec3_diff(v, transf.t)));
 87 }
 88 
 89 Vec3    trans3_vec(Transform3 transf, Vec3 v)
 90 {
 91     return (mat3_vprod(transf.R, v));
 92 }
 93 
 94 Vec3    trans3_inverse_vec(Transform3 transf, Vec3 v)
 95 {
 96     return (mat3_transpose_vprod(transf.R, v));
 97 }
 98 
 99 void    trans3_get_frame1in2(Transform3 transf, Vec3 * p, Vec3 * ex, Vec3 * ey, Vec3 * ez)
100 {
101     *p = transf.t;
102     *ex = mat3_colx(transf.R);
103     *ey = mat3_coly(transf.R);
104     *ez = mat3_colz(transf.R);
105 }
106 
107 void    trans3_get_frame2in1(Transform3 transf, Vec3 * p, Vec3 * ex, Vec3 * ey, Vec3 * ez)
108 {
109     *p = mat3_transpose_vprod(transf.R, vec3_minus(transf.t));
110     *ex = mat3_rowx(transf.R);
111     *ey = mat3_rowy(transf.R);
112     *ez = mat3_rowz(transf.R);
113 }
114 
115 Vec3    trans3_get_origin2in1(Transform3 transf)
116 {
117     return (mat3_transpose_vprod(transf.R, vec3_minus(transf.t)));
118 }
119 
120 /** compute transform from given frame to current frame **/
121 
122 Transform3 trans3_from_frame(Vec3 p, Vec3 ex, Vec3 ey, Vec3 ez)
123 {
124     Transform3 transf = {Transform3_id};
125 
126     transf.t = p;
127     transf.R = mat3_of_cols(ex, ey, ez);
128     return (transf);
129 }
130 
131 /** compute transform to given frame from current frame **/
132 
133 Transform3 trans3_to_frame(Vec3 p, Vec3 ex, Vec3 ey, Vec3 ez)
134 {
135     return (trans3_inverse(trans3_from_frame(p, ex, ey, ez)));
136 }
137 
138 Transform3 trans3_from_frame_to_frame(Vec3 p1, Vec3 ex1, Vec3 ey1, Vec3 ez1, Vec3 p2, Vec3 ex2, Vec3 ey2, Vec3 ez2)
139 {
140     Transform3 t1 = {Transform3_id};
141     Transform3 t2 = {Transform3_id};
142 
143     t1 = trans3_from_frame(p1, ex1, ey1, ez1);
144     t2 = trans3_to_frame(p2, ex2, ey2, ez2);
145     return (trans3_prod(t1, t2));
146 }
147 
148 void    trans3_print(FILE * fp, Transform3 trans)
149 {
150     mat3_print(fp, trans.R);
151     vec3_print(fp, trans.t);
152 }
153 
154 void    trans3_format(Transform3 trans)
155 {
156     mat3_format(trans.R);
157     format("\n");
158     vec3_format(trans.t);
159 }
160 
161 /**
162 3D rotation r about point p
163 
164 (leaves p unchanged)
165 **/
166 
167 Transform3 trans3_rot_about(Mat3 r, Vec3 p)
168 {
169     Transform3 transf = {Transform3_id};
170 
171     transf.R = r;
172     transf.t = vec3_diff(p, mat3_vprod(transf.R, p));
173     return (transf);
174 }
175 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.