1 /**********
2 *
3 * Copyright (c) 2003, Division of Imaging Science and Biomedical Engineering,
4 * University of Manchester, UK. All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without modification,
7 * are permitted provided that the following conditions are met:
8 *
9 * . Redistributions of source code must retain the above copyright notice,
10 * this list of conditions and the following disclaimer.
11 *
12 * . Redistributions in binary form must reproduce the above copyright notice,
13 * this list of conditions and the following disclaimer in the documentation
14 * and/or other materials provided with the distribution.
15 *
16 * . Neither the name of the University of Manchester nor the names of its
17 * contributors may be used to endorse or promote products derived from this
18 * software without specific prior written permission.
19 *
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 * POSSIBILITY OF SUCH DAMAGE.
32 *
33 **********
34 *
35 * Program : TINA
36 * File : $Source: /home/tina/cvs/tina-libs/tina/math/mathTran_transform3.c,v $
37 * Date : $Date: 2003/09/23 11:32:20 $
38 * Version : $Revision: 1.4 $
39 * CVS Id : $Id: mathTran_transform3.c,v 1.4 2003/09/23 11:32:20 matts Exp $
40 *
41 * Author : Legacy TINA
42 *
43 * Notes :Transforms
44 * Transform3 is {int type; struct mat3 R; struct vec3 t;}
45 *
46 *********
47 */
48 /**
49 * @file
50 * @brief Transform3 handling functions.
51 *
52 * Transform2 is {int type; struct mat3 R; struct vec3 t;}, ie, contains a 3D rotation
53 * matrix (Mat3) and a 3-element translation vector (Vec3).
54 *
55 * Functions include;
56 *
57 * - allocating/freeing/copying.
58 * - geometric uses of Transform3.
59 * - frame to frame transform calculations.
60 * - printing/formatting functions.
61 */
62 #include "mathTran_transform3.h"
63
64 #if HAVE_CONFIG_H
65 #include <config.h>
66 #endif
67
68 #include <stdio.h>
69 #include <math.h>
70 #include <tina/sys/sysDef.h>
71 #include <tina/sys/sysPro.h>
72 #include <tina/math/math_GeomDef.h>
73 #include <tina/math/math_GeomPro.h>
74 #include <tina/math/math_TranDef.h>
75
76 Transform3 trans3(Mat3 R, Vec3 t)
77 {
78 Transform3 transf = {Transform3_id};
79
80 transf.R = R;
81 transf.t = t;
82 return (transf);
83 }
84
85 Transform3 *trans3_alloc(void)
86 {
87 Transform3 *transf = ts_ralloc(Transform3);
88
89 transf->R = mat3_unit();
90 transf->t = vec3_zero();
91 return (transf);
92 }
93
94 Transform3 *trans3_make(Mat3 R, Vec3 t)
95 {
96 Transform3 *transf = ts_ralloc(Transform3);
97
98 transf->R = R;
99 transf->t = t;
100 return (transf);
101 }
102
103 Transform3 *trans3_copy(Transform3 * transf)
104 {
105 if (transf == NULL)
106 return (NULL);
107
108 return (trans3_make(transf->R, transf->t));
109 }
110
111 void trans3_free(void *transf)
112 {
113 rfree((void *) transf);
114 }
115
116 Transform3 trans3_unit(void)
117 {
118 Transform3 transf = {Transform3_id};
119
120 transf.R = mat3_unit();
121 transf.t = vec3_zero();
122 return (transf);
123 }
124
125 Transform3 trans3_prod(Transform3 transf2, Transform3 transf1)
126 {
127 Transform3 prod = {Transform3_id};
128
129 prod.R = mat3_prod(transf2.R, transf1.R);
130 prod.t = vec3_sum(transf2.t, mat3_vprod(transf2.R, transf1.t));
131 return (prod);
132 }
133
134 Transform3 trans3_inverse(Transform3 transf)
135 {
136 Transform3 inv = {Transform3_id};
137
138 inv.R = mat3_transpose(transf.R);
139 inv.t = mat3_vprod(inv.R, vec3_minus(transf.t));
140 return (inv);
141 }
142
143 Vec3 trans3_pos(Transform3 transf, Vec3 v)
144 {
145 return (vec3_sum(mat3_vprod(transf.R, v), transf.t));
146 }
147
148 Vec3 trans3_inverse_pos(Transform3 transf, Vec3 v)
149 {
150 return (mat3_transpose_vprod(transf.R, vec3_diff(v, transf.t)));
151 }
152
153 Vec3 trans3_vec(Transform3 transf, Vec3 v)
154 {
155 return (mat3_vprod(transf.R, v));
156 }
157
158 Vec3 trans3_inverse_vec(Transform3 transf, Vec3 v)
159 {
160 return (mat3_transpose_vprod(transf.R, v));
161 }
162
163 void trans3_get_frame1in2(Transform3 transf, Vec3 * p, Vec3 * ex, Vec3 * ey, Vec3 * ez)
164 {
165 *p = transf.t;
166 *ex = mat3_colx(transf.R);
167 *ey = mat3_coly(transf.R);
168 *ez = mat3_colz(transf.R);
169 }
170
171 void trans3_get_frame2in1(Transform3 transf, Vec3 * p, Vec3 * ex, Vec3 * ey, Vec3 * ez)
172 {
173 *p = mat3_transpose_vprod(transf.R, vec3_minus(transf.t));
174 *ex = mat3_rowx(transf.R);
175 *ey = mat3_rowy(transf.R);
176 *ez = mat3_rowz(transf.R);
177 }
178
179 Vec3 trans3_get_origin2in1(Transform3 transf)
180 {
181 return (mat3_transpose_vprod(transf.R, vec3_minus(transf.t)));
182 }
183
184 /** compute transform from given frame to current frame **/
185
186 Transform3 trans3_from_frame(Vec3 p, Vec3 ex, Vec3 ey, Vec3 ez)
187 {
188 Transform3 transf = {Transform3_id};
189
190 transf.t = p;
191 transf.R = mat3_of_cols(ex, ey, ez);
192 return (transf);
193 }
194
195 /** compute transform to given frame from current frame **/
196
197 Transform3 trans3_to_frame(Vec3 p, Vec3 ex, Vec3 ey, Vec3 ez)
198 {
199 return (trans3_inverse(trans3_from_frame(p, ex, ey, ez)));
200 }
201
202 Transform3 trans3_from_frame_to_frame(Vec3 p1, Vec3 ex1, Vec3 ey1, Vec3 ez1, Vec3 p2, Vec3 ex2, Vec3 ey2, Vec3 ez2)
203 {
204 Transform3 t1 = {Transform3_id};
205 Transform3 t2 = {Transform3_id};
206
207 t1 = trans3_from_frame(p1, ex1, ey1, ez1);
208 t2 = trans3_to_frame(p2, ex2, ey2, ez2);
209 return (trans3_prod(t1, t2));
210 }
211
212 void trans3_print(FILE * fp, Transform3 trans)
213 {
214 mat3_print(fp, trans.R);
215 vec3_print(fp, trans.t);
216 }
217
218 void trans3_format(Transform3 trans)
219 {
220 mat3_format(trans.R);
221 format("\n");
222 vec3_format(trans.t);
223 }
224
225 /**
226 3D rotation r about point p
227
228 (leaves p unchanged)
229 **/
230
231 Transform3 trans3_rot_about(Mat3 r, Vec3 p)
232 {
233 Transform3 transf = {Transform3_id};
234
235 transf.R = r;
236 transf.t = vec3_diff(p, mat3_vprod(transf.R, p));
237 return (transf);
238 }
239
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.